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Abstract 

The  fundamental  requirement  of  truly  autonomous  mobile  robots  is  navigation.  Nav¬ 
igation  is  the  science  of  determining  one’s  position  and  orientation  based  on  infor¬ 
mation  provided  by  various  sensors.  Mobile  robot  navigation,  especially  autonomous 
vehicle  navigation,  is  confronted  with  the  problem  of  attempting  to  determine  the 
structure  of  an  a  priori  unknown  environment,  while  at  the  same  time  using  this  in¬ 
formation  for  navigation  purposes.  This  problem  is  referred  to  as  concurrent  mapping 
and  localization  (CML).  This  thesis  addresses  the  question  of  how  to  improve  CML 
performance  through  smarter  sensing  strategies  affecting  robot  behavior.  Planned 
perception  is  the  process  of  adaptively  determining  the  sensing  strategy  of  the  mo¬ 
bile  robot.  The  goal  of  integrating  planned  perception  within  concurrent  mapping 
and  localization  is  to  attempt  to  answer  the  question  of  how  a  mobile  robot  should 
behave  so  as  to  attempt  to  optimize  CML  performance.  This  thesis  demonstrates  in 
simulation  how  the  CML  framework  could  be  improved  with  planned  perception  by 
motivating  changes  in  robot  pose  and  hence,  sensing  locale. 
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Chapter  1 


Introduction 


Navigation  is  the  science  of  determining  one’s  position  and  orientation  based  on  infor¬ 
mation  provided  by  various  sensors.  Mobile  robot  navigation,  especially  autonomous 
vehicle  navigation,  is  confronted  with  the  problem  of  attempting  to  determine  the 
structure  of  an  a  priori  unknown  environment,  while  at  the  same  time  using  this 
information  for  navigation  purposes.  Overcoming  this  problem  is  essential  for  true 
autonomy  [13,26].  This  problem  is  challenging  because  it  must  address  two  difficult 
issues  simultaneously:  navigation  and  mapping  [2,7,27,46].  Concurrent  Mapping 
and  Localization  (CML)  is  the  process  of  simultaneously  building  a  map  of  the  en¬ 
vironment  and  using  this  map  to  obtain  improved  estimates  of  the  location  of  the 
vehicle. 

The  fundamental  requirement  of  truly  autonomous  mobile  robots  is  navigation  [21]; 
precision  underwater  vehicle  navigation  remains  the  principle  obstacle  to  improved 
mobile  robot  and  autonomous  underwater  vehicle  (AUV)  control  [52].  Concurrent 
mapping  and  localization  is  intended  to  enable  a  mobile  robot  faced  with  the  tasks 
of  mapping  and  navigating  to  address  these  issues  concurrently  and  reliably. 

Autonomous  vehicles  must  address  the  tasks  associated  with  concurrent  mapping 
and  localization.  This  thesis  addresses  the  question  of  how  the  incorporation  of 
smarter  sensing  strategies  will  improve  CML  performance.  Through  the  integration 
of  planned  perception  with  CML  we  will  be  motivated  to  change  robot  pose  and 
hence,  sensing  locale.  Adaptive  sensing  through  planned  perception  will  maximize 
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robot  pose  certainty  through  the  re-observation  of  features  of  importance. 


1.1  Navigation  techniques  used  in  CML 

The  localization  problem  is  the  specific  problem  of  determining  the  location  of  a 
robot  relative  to  a  map  [50].  Accurate  localization  forms  the  basis  for  most  control 
and  navigation  decisions;  without  this  feedback,  control  performance  is  limited  [53]. 
Thus,  reliable  localization  is  an  essential  component  of  any  autonomous  vehicle  sys¬ 
tem  [24, 27, 51].  CML  approaches  the  navigation  problem  with  three  primary  repre¬ 
sentations;  beacon-based  navigation,  relative  navigation  (dead  reckoning  and  inertial 
navigation  systems),  and  map-based  navigation.  Relative  navigation  is  subject  to  ex¬ 
ternal  disturbances  and  uncorrectable  drift  allowing  position  errors  to  grow  without 
bounds.  However,  bounded  errors  may  be  achieved  through  acoustic  transponder  re¬ 
sets.  Regardless,  CML  has  the  potential  to  enable  missions  with  bounded  navigation 
errors  without  relying  on  a  priori  maps,  acoustic  beacons,  or  GPS  resets  [20,27]. 


1.1.1  Dead  reckoning  and  inertial  navigation  systems 

Dead  reckoning  is  the  traditional  and  most  common  navigation  technique.  Estimates 
of  vehicle  position  are  obtained  by  integrating  the  vehicle  velocity  over  time.  Inertial 
navigation  systems  (INS)  integrate  the  vehicle’s  acceleration  twice  to  obtain  new 
position  estimates.  Vehicle  motion  is  sensed  through  gyroscopes  and  accelerometers. 
Starting  with  the  most  recent  estimate  of  vehicle  speed  and  direction,  these  integrated 
quantities  are  used  to  achieve  updates  of  new  position  estimates. 

Although  dead  reckoning  is  a  common  navigation  technique,  it  has  certain  draw¬ 
backs.  Navigation  is  limited  by  inaccuracies  resulting  from  integration  errors  and 
system  biases.  Also,  inertial  navigation  systems  are  expensive  and  consume  much 
power.  Relying  exclusively  on  dead  reckoning  results  in  position  error  that  grows 
without  bound  over  time. 
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1.1.2  Beacon-based  navigation 

Beacons  placed  at  known  locations  allow  for  vehicles  to  determine  their  position 
through  triangulation  [34].  The  beacons  emit  pulses  utilizing  transducers  or  trans¬ 
ducer  arrays.  Position  fixes  of  vehicle  location  may  be  obtained  by  detecting  these 
outgoing  pulses  and  triangulating  position  location  based  on  range  and/or  bearing 
measurements  given  a  priori  knowledge  of  beacon  locations. 

The  satellite-based  Global  Positioning  System  (GPS)  is  the  most  prevalent  beacon- 
based  navigation  system.  This  system,  for  many  outdoor  applications,  provides  a 
means  for  estimating  position  with  a  high  accuracy.  GPS  signals  are  unable  to  be 
utilized  indoors  or  underwater  because  of  signal  attenuation.  Therefore,  in  the  areas 
of  indoor  and  underwater  mobile  robotics,  GPS  is  not  a  reliable  resource.  Currently, 
two  primary  acoustic  transponder  systems  are  used  in  underwater  navigation:  long 
baseline  (LBL)  and  ultra-short  baseline  (USBL)  [34].  Both  the  USBL  and  LBL  sys¬ 
tems  rely  on  accurate  beacon  positioning  in  order  to  obtain  vehicle  position  relative 
to  the  transponders  or  beacons  as  shown  in  Figurel-1. 


1.1.3  Map-based  navigation 

For  some  missions,  beacons  may  be  unavailable  or  impractical.  However,  if  an  a  priori 
map  of  the  environment  is  available,  it  may  be  possible  to  navigate  relative  to  terrain. 
Map-based  navigation  is  performed  by  correlating  sensor  data  with  an  a  priori  map  to 
deduce  accurate  localization.  One  such  system  is  used  by  the  U.S.  Navy  Tomahawk 
Cruise  Missile  (BGM-109).  The  Tomahawk’s  system  couples  inertial  guidance  with  a 
terrain  contour  matching  (TERCOM)  guidance  system  [39].  By  comparing  a  stored 
map  to  actual  terrain  measurements,  it  is  possible  to  estimate  the  missile’s  position. 

Unclassified,  accurate,  underwater  maps  are  not  often  available.  Even  if  a  prior 
map  exists,  matching  the  sensed  environment  to  that  of  the  given  prior  map  is  a 
challenge.  Map- based  navigation  has  been  applied  by  Carpenter  and  Medeiros  [10]. 
They  performed  map-based  navigation  onboard  an  AUV  with  bathymetric  data. 
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Figure  1-1;  Basic  LBL  Description.  The  vehicle  estimates  its  position  by  obtaining 
a  “fix.”  A  fix  is  obtained  by  triangulating  range  measurements  received  from  each 
beacon. 


1.2  Mapping  techniques  used  in  CML 

Because  a  priori  known  maps  of  the  environment  are  not  always  available,  robots 
often  must  build  such  a  map  themselves.  The  task  of  building  a  map  is  estimating  the 
locations  of  unknown  objects  in  the  environment  [29].  Once  a  map  of  the  environment 
is  provided,  accurate  vehicle  localization  can  be  derived.  Approaches  to  map  creation 
can  be  categorized  into  three  main  areas:  grid-based  mapping,  feature-based  mapping, 
and  topological  mapping. 

1.2.1  Grid-based  mapping 

Grid-based  map  representations,  such  as  those  implemented  by  Moravec  [35]  and 
Thrun  et  al  [48],  represent  the  environment  as  an  evenly-spaced  grid.  Individual 
cells  that  are  certain  to  be  occupied  by  a  feature  will  be  assigned  a  probability  value 
of  1.  Cells  that  are  sure  to  be  free  of  features  are  assigned  probability  values  of  0.  A 
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map  of  the  environment  is  formed  based  on  the  assigned  probabilities  and  is  referred 
to  as  a  certainty  grid  or  occupancy  grid.  Mapping  is  performed  by  incorporating  new 
measurements  of  the  surrounding  environment  into  the  certainty,  grid.  The  probabil¬ 
ity  values  of  the  respective  grid  cells  are  adjusted  according  to  the  information  and 
certainty  of  object  locations  obtained  from  the  vehicle’s  measurements.  The  robot 
performs  localization  through  map  matching.  Map  matching  is  the  process  in  which 
the  robot  creates  a  new  local  map  and  then  compares  this  map  to  all  previously 
constructed  global  maps.  The  best  map  match  is  found  through  the  correlation  of 
the  local  map  with  previous  global  maps.  It  is  from  this  match  that  the  new  esti¬ 
mate  of  the  robot’s  location  is  determined.  Current  state-of-the-art  implementation 
of  grid-based  mapping  has  been  performed  by  Thrun  et  al  [9]. 

The  benefits  of  representing  the  environment  using  a  grid-based  map  include  sim¬ 
plicity  of  implementation  and  maintenance.  However,  this  approach  has  a  weak  the¬ 
oretical  foundation  and  some  feature-specific  information  (such  as  type  of  feature) 
is  lost  when  features  are  assigned  as  probabilities  to  grid  cells  [21,22,29].  Other 
drawbacks  to  grid-based  approaches  are  high  storage  requirements  for  data,  difficulty 
in  differentiating  similar  environments,  and  a  high  computational  cost  of  localiza¬ 
tion  [49]. 


1.2.2  Feature-based  mapping 

Rooted  in  the  ideas  of  target  tracking  and  surveillance  [3],  a  geometric,  feature- 
based  representation  of  the  map  models  the  environment  as  a  set  of  geometric  primi- 
tives(such  as  points,  lines,  and  planes)  and  builds  a  metrically  accurate  map  to  encode 
the  landmark  features  [28,29].  With  the  introduction  of  Stochastic  Mapping  (SM), 
Smith,  Self,  and  Cheeseman  [46]  paved  the  way  for  future  work  in  the  field  of  feature- 
based  mapping.  Stochastic  mapping,  presented  in  more  detail  in  Chapter  2,  provides 
the  theoretical  foundation  for  the  implementation  of  feature-based  CML. 
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1.2.3  Topological  mapping 

Instead  of  representing  the  environment  as  a  metrically  accurate  map,  topological 
mapping  generates  graph-like  descriptions  of  the  environment.  Kuipers  and  Byun  [25] 
have  used  the  topological  approach  by  representing  the  environment  as  a  graph  of 
arcs  and  nodes  [16].  Nodes  and  arcs  represent  different  aspects  of  the  environment 
map.  Nodes  represent  easily  distinguishable  “significant  places”  in  the  environment. 
Arcs  connect  the  nodes  and  represent  the  set  of  actions  that  connect  the  significant 
places.  This  topological  model  is  behavior-based  and  utilizes  reactive  rules  to  move 
between  the  nodes,  thus  it  is  able  to  be  used  for  route  planning  and  obstacle  avoidance. 
The  drawbacks  to  using  a  topological  approach  to  mapping  are  applications  to  larger 
environments  and  “significant  place”  recognition  [25].  Because  globally  referenced, 
metrically  accurate  maps  are  a  necessity  for  many  operations,  these  drawbacks  may 
pose  a  significant  threat  to  achieving  mission  objectives  [21]. 

1.3  Feature-based  CML 

Motivated  by  the  need  for  accurate,  reliable  navigation  systems  for  AUVs,  this  study 
of  mobile  robots  pertains  to  autonomous  vehicles  overcoming  the  problem  of  oper¬ 
ating  in  an  unknown  environment  with  imprecise  navigation  properties.  The  CML 
algorithm  allows  the  vehicle,  starting  at  an  unknown  location,  to  build  a  map  of  the 
previously  unknown  environment  and  then  utilize  that  information  to  improve  its 
own  navigation  estimate  [44].  The  research  presented  in  this  thesis  concentrates  on 
feature-based  CML. 

This  thesis  focuses  on  the  feature-based  approach  of  Stochastic  Mapping  (SM). 
Stochastic  mapping  [45]  provides  the  theoretical  foundation  for  the  implementation  of 
feature-based  CML.  First  introduced  by  Smith,  Self,  and  Cheeseman  [46],  stochastic 
mapping  uses  the  extended  Kalman  filter  (EKF)  [4, 43]  for  state  estimation.  This  algo¬ 
rithm  stores  estimates  of  robot  and  environmental  feature  locations  and  orientations 
in  a  single  state  vector  along  with  an  associated  error  covariance  matrix  representing 
the  correlations  between  all  mapped  entities  [41],  Features  may  be  added  to  and  re- 
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moved  from  the  state  vector  and  covariance  matrix  as  time  progresses;  as  new  features 
are  observed,  the  state  space  is  augmented.  The  stochastic  mapping  algorithm  is  used 
to  build  and  update  a  feature-based  map  of  the  environment  from  observations  (sensor 
measurements)  and  from  a  model  of  vehicle  dynamics/kinematics.  Simultaneously, 
like  the  EKF,  SM  performs  localization  by  updating  vehicle  position. 

One  challenge  to  stochastic  mapping  is  data  association.  The  data  association 
problem  consists  of  relating  observations  and  measurements  with  the  corresponding 
features  [31,44].  Data  association  and  obtaining  a  correct  solution  is  crucial  because 
a  misassignment  will  cause  the  filter  estimates  to  diverge  [37].  Solutions  to  the  data 
association  problem  must  be  addressed  to  employ  a  stochastic  mapping  approach  to 
CML  [20]. 

Stochastic  mapping  and  extensions  to  stochastic  mapping  have  proven  to  yield 
valid  solutions  to  the  concurrent  mapping  and  localization  problem.  Some  solutions 
have  expanded  SM  and  incorporate  track  initiation,  track  deletion,  and  data  associa¬ 
tion  [17-19,27,44,53].  Dissanayake  [17]  and  Newman  [38]  provide  an  analysis  of  the 
performance  of  the  stochastic  mapping  algorithm.  These  results  show  that  a  solution 
to  the  CML  problem  is  possible;  an  autonomous  vehicle  located  in  an  unknown  po¬ 
sition  in  an  unknown  environment  can  build  a  map,  using  only  observations  relative 
to  its  position,  and  simultaneously  compute  a  bounded  estimate  of  vehicle  location. 

The  strengths  and  advantages  of  the  feature-based  approach  of  Stochastic  Map¬ 
ping  to  the  CML  problem  are: 

•  Easily  identifiable  features  in  the  environment  are  able  to  be  extracted  and 
mapped. 

•  A  recursive  solution  to  the  navigation  problem  is  provided. 

•  Consistent  estimates  for  uncertainty  in  vehicle  and  feature  locations  are  com¬ 
puted. 

•  SM  can  provide  robust  globally  referenced  navigation  information. 
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•  The  SM  approach  to  CML  has  the  potential  to  enable  autonomous  robots  to 
operate  with  bounded  navigation  errors  without  relying  on  acoustic  beacons,  a 
priori  maps,  or  GPS  updates. 


1.4  Problem  Statement 

Stochastic  mapping  is  a  viable  solution  to  the  problem  of  operating  in  an  unknown 
location  in  an  unknown  environment.  The  feature-based  approach  of  CML  utilizing 
stochastic  mapping  requires  a  mobile  robot  to  take  observations  of  its  environment. 
The  robot  then  maps  its  surrounding  features  and  uses  this  map  to  navigate.  Planned 
perception  involves  the  focusing  of  sensory  efforts  to  selected  areas  of  interest.  Areas 
of  interest  are  defined  as  those  areas  and  features  which  will  improve  the  current 
navigation  estimates  and/or  those  which  may  contain  new,  useful  features. 

Planned  perception  is  the  process  of  adaptively  determining  the  sensing  strat- 
egy  of  the  mobile  robot.  The  goal  of  integrating  planned  perception  with  CML  is  to 
provide  the  mobile  robot  with  a  means  to  determine  the  optimal  action  given  the  cur¬ 
rent  knowledge  of  robot  pose  (attitude  and  position),  the  environment,  and  sensors. 
Planned  perception  is  a  step  in  the  direction  of  improving  the  overall  CML  frame¬ 
work.  Strategic  sensing  will  allow  for  a  mobile  robot  to  limit  its  area  of  sensing  to 
those  of  interest  or  those  that  will  provide  a  means  of  minimizing  vehicle  uncertainty. 

By  integrating  CML  with  planned  perception  through  smarter  sensing  strategies 
we  will  attempt  to  answer  the  question  of  how  a  mobile  vehicle  should  behave  so  as  to 
attempt  to  maximize  CML  performance.  CML  performance  could  be  defined  as  dis¬ 
covering  areas  of  interest,  maximizing  pose  certainty,  and  maximizing  map  (feature) 
certainty. 

This  thesis  will  demonstrate  in  simulation  how  the  CML  framework  could  be 
improved  through  smarter  sensing  strategies  and  how  a  mobile  vehicle  should  behave 
so  as  to  attempt  to  optimize  CML  performance.  The  proposed  approach  will  utilize 
a  metric  of  feature  quality  and  feature  modality  applied  to  robot  sensing  to  motivate 
changes  in  robot  behavior.  We  intend  to  show  that  CML  performance,  augmented 
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with  smarter  sensing  strategies,  will  exhibit  improved  performance  by  motivating 
changes  in  robot  orientation  and  sensing  strategies. 


1.5  Summary 

The  chapter  presented  the  concept  of  concurrent  mapping  and  localization  as  well  as 
the  idea  of  integrating  planned  perception  with  CML.  The  basic  description  of  the 
current  techniques  of  autonomous  underwater  vehicle  mapping  and  navigation  were 
presented  and  reviewed.  This  thesis  focuses  on  the  problem  of  extending  feature- 
based  SM  and  concurrent  mapping  and  localization  through  the  application  of  smarter 
sensing  strategies  which  will  affect  the  mobile  vehicle’s  behavior. 


1.6  Thesis  contributions 

This  thesis  makes  the  following  contributions: 

•  A  method  for  integrating  planned  perception  within  concurrent  mapping  and 
localization. 

•  An  analysis  of  planned  perception  performance  in  simulation. 


1.7  Thesis  overview 

This  thesis  presents  an  algorithm  demonstrating  how  the  CML  framework  could  be 
improved  through  smarter  sensing  strategies  and  how  a  mobile  vehicle  should  behave 
so  as  to  attempt  to  optimize  CML  performance.  The  structure  of  this  thesis  is  as 
follows: 

Chapter  2  establishes  the  mathematical  framework  employed  in  the  study  of  the 
concurrent  mapping  and  localization  problem. 

Chapter  3  presents  the  theoretical  contribution  of  this  thesis  as  it  addresses  the 
integration  of  planned  perception  within  the  stochastic  mapping  algorithm. 
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Chapter  4  applies  the  integration  of  planned  perception  and  CML.  The  design 
and  results  of  the  simulation  are  presented. 

Chapter  5  summarizes  the  main  contributions  of  this  thesis  and  provides  sugges¬ 
tions  for  future  research. 


Chapter  2 


Stochastic  Mapping 


This  chapter  is  concerned  with  the  mathematical  framework  employed  in  this  study 
of  Concurrent  Mapping  and  Localization.  This  framework  was  first  introduced  in 
a  seminal  paper  [46]  by  Smith,  Self,  and  Cheeseman  and  is  known  as  Stochastic 
Mapping.  Stochastic  Mapping  (SM)  is  simply  a  way  of  implementing  feature-based 
concurrent  mapping  and  localization  utilizing  an  extended  Kalman  filter  for  state 
estimation  [21]. 

This  chapter  reviews  the  stochastic  mapping  algorithm.  It  begins  with  a  brief 
overview  of  the  Kalman  filter.  Section  2.2  defines  the  state  vector  and  covariance 
matrix  which  are  used  to  describe  the  system  behavior.  Section  2.3  presents  the 
models  employed  to  represent  a  mobile  robot  and  its  environment  in  order  to  solve 
the  CML  problem.  A  generalized  framework  for  the  SM  estimation  process  is  then 
presented  in  Section  2.4.  The  chapter  concludes  with  an  analysis  of  structure  of 
the  CML  problem  and  the  convergence  properties  of  the  map  and  its  steady-state 
behavior. 


2.1  The  Kalman  Filter 

In  this  section  we  will  outline  the  Kalman  filter  and  the  extended  Kalman  filter.  For 
a  full  derivation  and  a  more  detailed  discussion  refer  to  [4, 8, 23, 33, 43].  The  Kalman 
filter  is  introduced  because  it  serves  as  the  premise  for  the  extended  Kalman  filter 
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(EKF).  The  EKF  is  the  observer  [5]  used  in  the  stochastic  mapping  approach  to  CML. 

2.1.1  The  Kalman  filter 

The  Kalman  filter  is  a  recursive  least  squares  estimator.  It  is  the  optimal  estimator 
if  the  dynamic  model  is  linear  and  the  noise  processes  are  Gaussian.  If  the  noise 
processes  are  non-Gaussian  but  the  system  is  linear,  the  Kalman  filter  is  the  best 
linear  estimator  [4].  Without  the  loss  of  generality,  in  this  discussion  of  the  Kalman 
filter,  a  linear  dynamic  system  and  Gaussian  noise  processes  are  assumed. 

The  Kalman  filter  is  a  way  of  determining  minimum  mean-square  error  (MMSE) 
estimates  using  state-space  methods.  It  is  a  procedure  that  uses  the  results  of  the 
previous  time  step  to  aid  in  obtaining  the  estimate  at  the  current  step  of  the  process  [8, 
43].  The  outline  of  the  Kalman  filter  is  seen  in  Figure  2-1. 

At  time  step  k  the  Kalman  filter  produces  an  MMSE  estimate  k{k\k)  of  the  state 
vector  x(A:).  This  estimate  summarizes  the  information  up  to  time  step  k-  1.  Refine¬ 
ment  of  our  estimate  is  obtained  by  fusing  a  prediction  of  the  state  estimate  x(fc|A:- 1) 
with  an  observation  z(fc)  of  the  state  vector  x(A:).  The  recursion  is  completed  when 
the  estimated  state  x(/e|/r)  is  predicted  through  a  system  model  to  produce  a  new 
estimate  x(A:  +  l|/c). 

The  Kalman  filter  is  a  recursive  solution  of  the  linear  filtering  problem.  Despite 
the  Kalman  filter’s  efficiency,  real  navigation  problems  almost  always  involves  a  non¬ 
linear  dynamic  system.  The  extended  Kalman  filter  is  used  in  situations  involving 
non-linear  vehicle  motion  and  measurement  relationships. 


2.1.2  The  Extended  Kalman  filter 

In  any  real  navigation  problem  vehicle  motion  and  the  observation  of  features  are 
almost  always  a  non-linear  processes.  To  compensate  for  the  non-linearity  of  the 
dynamic  system,  two  basic  ways  of  linearizing  the  non-linearities  are  the  linearized 
Kalman  filter  and  the  extended  Kalman  filter  [8].  The  linearized  Kalman  filter  re¬ 
quires  linearizing  about  some  nominal  trajectory  in  state  space  that  does  not  depend 
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Figure  2-1:  The  Kalman  Filter.  From  Bar-Shalom  and  Li  [4] 
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on  measurement  data.  The  extended  Kalman  filter  (EKF)  linearizes  about  a  trajec¬ 
tory  that  is  updated  continually  with  the  state  estimates  resulting  from  observations. 
The  EKF  is  the  core  method  used  in  stochastic  mapping.  It  is  the  technique  of 
linearizing  a  non-linear  dynamic  system  for  use  in  a  Kalman  filter. 

In  order  to  circumvent  the  problem  of  operating  with  non-linearities,  the  non¬ 
linear  models  are  approximated  through  a  Taylor  series  expansion.  The  first  order 
version  of  this  expansion  allows  for  the  filter  to  be  derived  in  the  same  manner  as 
for  the  linear  Kalman  filter  with  the  exception  that  the  non-linear  vehicle  model  and 
observation  model  are  linearized  [4]. 


This  linearization  is  performed  using  Jacobians.  The  Jacobian  of  a  function  is  a 
matrix  of  partial  derivatives  with  respect  to  a  vector.  The  Jacobian  of  a  function  f 
with  vector  x  is  defined  as 


fx  =  Vf  = 


ax„ 


afi 

Ml 

0Xi 

dXn 

df2 

Ml. 

dh 

dxi 

dx2 

dXn 

dfrrr 

Mul  .  . 

dfm 

dxi 

dX2 

dXn 

(2.1) 


Assuming  that  the  approximation  error  of  linearizing  the  non-linearities  is  small, 
the  EKF  can  then  be  derived  in  the  same  manner  as  the  linear  case.  The  outline  of 
the  extended  Kalman  filter  is  seen  in  Figure  2-2. 


It  is  the  extended  Kalman  filter  that  forms  the  basis  for  the  stochastic  mapping 
algorithm.  Smith,  Self,  and  Cheeseman  state  that  the  reasons  for  using  EKF  are 
because  of  its  simplicity  in  implementation,  its  similarity  to  the  optimal  linear  filter 
(linear  Kalman  filter),  and  its  ability  to  provide  accurate  estimates  [46].  The  EKF 
provides  a  means  for  which  a  mobile  robot  can  perform  relative  measurements  to  fea¬ 
tures  and  obtain  a  bounded  estimate  of  vehicle  and  landmark  locations  in  a  recursive 
fashion. 
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Figure  2-2:  The  Extended  Kalman  Filter.  From  Bar-Shalom  and  Li  [4] 
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2.2  State  vector  and  covariance  matrix 


Stochastic  mapping  represents  the  environmental  map  as  a  system  state  vector  and  an 
associated  estimate  error  covariance.  The  state  vector,  x,  is  defined  as  a  combination 
of  the  vehicle  states,  x^,  and  feature  states,  x/.  It  stores  the  estimated  state  of  the 
environment  (feature  locations)  and  the  state  of  the  vehicle  (robot  pose).  The  state 
of  the  system  at  time  k  can  be  represented  by  the  augmented  state  vector,  x{k). 
Where  Nf  describes  the  number  of  observed  features  at  time  k,  Xf.{k)  =  [x/^ 
i=  the  system  state  vector  is  defined 


x{k) 


Xy{k) 

Xf{k) 


Xy{k) 

Xf^{k) 


(2.2) 


The  SM  algorithm  is  a  recursive  estimation  process  that  produces  a  MMSE  es¬ 
timate  x{k  +  1|A;)  of  the  state  x  given  a  sequence  of  observations  up  to  time  k+  1, 
Z*  =  {z(l), . . .  ,z(A;)}.  The  filter  fuses  a  prior  estimate  x{k\k  -  1)  with  an  observa¬ 
tion  z{k)  of  state  x(A:)  at  time  k  to  produce  an  updated  estimate  x(A:|A:).  The  state 
estimate  is  defined 


x(A:-bliA:)  =  E[x(A:-M)|Z'=] 
Xy{k  +  1|A:) 

Xfiik  +  1|A:) 

^fNf{k  +  1|A;) 


(2.3) 


(2.4) 


Throughout  this  thesis,  the  vehicle’s  state  estimate  will  be  defined  by 
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Figure  2-3:  Definitions  of  the  vehicle  states  used  in  the  model 


to  represent  the  vehicle’s  east  position,  north  position,  and  heading  as  shown  in 
Figure  2-3. 

The  estimated  error  covariance  associated  with  the  state  vector  is  given  by 

P{k\k)  =  E[(x(A:)  -  ±{k\k)){x{k)  -  x(/c|fc))^|Z''].  (2.6) 

This  matrix  defines  the  mean  squared  error  and  error  correlations  in  each  of  the  state 
estimates.  The  covariance  matrix  may  also  be  written  as 

F W  E-yl  *  *  *  PvNf 

Plv  Pll  •••  PlN/ 

P  NfV  P  Nfl  ‘  ‘  *  ^  NfNf 

The  sub-matrices,  P^^,  Pji,  and  P^i  are  the  vehicle-to- vehicle,  feature-to-feature,  and 
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vehicle-to-feature  covariances  respectively.  The  vehicle  {Pvv{k))  and  feature  (Pii(fc)) 
covariances  are  located  on  the  main  diagonal.  They  describe  the  uncertainty  in  the 
estimate  of  each  state.  The  off-diagonals  contain  the  vehicle-to-feature,  (P„j(A:)),  and 
feature-to-feature,  (Py(A:)),  cross  correlations.  The  off-diagonal  terms  describe  the 
correlation  between  the  uncertainties. 

It  is  essential  to  maintain  the  cross  correlations  for  two  reasons.  First,  the  infor¬ 
mation  gained  about  one  state  can  be  used  to  improve  the  estimate  of  other  correlated 
states.  Second,  maintaining  cross  correlation  terms  prevent  the  SM  algorithm  from 
becoming  “overconfident.”  Being  overconfident  leads  to  incorrectly  assuming  features 
are  independent  when  in  fact,  they  are  correlated  [11]. 

Thus,  the  vehicle  and  map  are  represented  by  a  single  state  vector  x  with  estimate 
X  and  an  associated  estimate  error  covariance  P.  Given  the  definitions  above,  an  EKF 
is  employed  to  estimate  the  state  and  covariance  given  the  measurements  z. 


2.3  System  models 

The  stochastic  mapping  algorithm  depends  on  three  key  models:  the  vehicle  model, 
the  feature  model,  and  the  observation  model.  This  section  presents  the  general  form 
of  these  models  employed  by  this  thesis.  The  vehicle  model  describes  the  kinematics 
and  dynamics  of  the  mobile  robot.  The  feature  model  describes  feature  physics  and 
relation  to  the  environment.  The  observation  model  relates  the  observations  to  the 
state  vector. 

2.3.1  Vehicle  model 

The  general  form  of  a  vehicle  model  (or  process  model)  can  be  written  as 

Xu(fc  -|-  1)  =  f„[x„(/!;),  Uy(^k  -|-  1),  (A:  -|-  1)]  -|-  Vy(^k  1).  (2.8) 

This  model  attempts  to  capture  the  relationship  between  the  vehicle’s  past  state, 
'x.y{k),  and  its  current  state,  x„(A:  -f  1),  given  a  control  input,  Ui,(A:  +  1). 
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The  vector  Xy{k)  is  the  vehicle  state  vector.  It  describes  the  state  of  the  vehicle 
at  time  k.  The  vehicle  state  vector  may  contain  any  number  of  vehicle  parameters. 
However,  for  the  purpose  of  this  thesis,  the  state  vector  is  limited  to  defining  the 
position  and  orientation  of  the  vehicle  in  two  dimensions. 

The  function  f„  is  the  state  transition  matrix.  It  mathematically  represents  the 
mobile  robot’s  dynamics,  mobility,  and  kinematics.  The  discrete  time  vehicle  model 
describes  the  transition  of  the  vehicle  state  vector  x„  from  time  k  to  time  k+1. 

The  vector  Uy{k  +  1)  is  the  control  input  at  time  k+1.  The  control  inputs  in  this 
thesis  are  vehicle  speed  and  heading  or  steer  angle,  defining  u  =  [u  0]^ . 

The  random  vector  v„  is  the  vehicle  model  process  noise.  It  represents  all  of  the 
noise  and  unmodelled  aspects  of  vehicle  behavior.  This  vehicle  model  process  noise  is 
assumed  to  be  a  stationary,  temporally  uncorrelated  zero  mean  Gaussian  white  noise 
process.  Because  of  this  assumption,  and  the  defined  expectation  in  Equation  2.9,  it 
can  be  shown  that  [8, 43] 


E[vy]=0yk 


(2.9) 


E[vw  ■  v„J] 


Q„(A;)  iii  =  j  =  k 
0  otherwise 

\ 


where  Qv{k)  is  the  covariance  of  v„  at  time  k  such  that 


(2.10) 


Q.  =  Q  = 


Vy  0 
0  (py 


(2.11) 


and  Vy  and  (py  are  the  variances  in  error  associated  with  velocity  and  heading,  respec¬ 
tively.  Q  is  the  uncertainty  in  the  process  noise  modelled  with  known  variances. 

The  vehicle  model  used  in  this  thesis  is  given  by  Equation  2.12 


■x.y{k  -I- 1)  =  fi,x„(fc)  +  Uy{k  +  1)  +  ^v{k  -f- 1).  (2.12) 


The  dynamic  model  fv  is  the  defined  as  the  state  transition  matrix  for  the  vehicle 
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model.  The  vehicle  model  used  in  this  thesis  can  be  expanded  and  written  as 


X.y{k  +  1)  = 


X  +  vAT  cos{6) 
y  +  vAT  sin(0)  , 
0  +  <p 


(2.13) 


where  AT  is  the  change  in  time.  This  particular  vehicle  model  is  non-linear.  The 
model  will  be  linearized  using  its  Jacobian  evaluated  at  the  vehicle  state  at  time  k 
such  that 


VUk)  = 


1  0  —  v{k)  AT  sin  ip{k) 
0  1  V {k)  AT  cos  (p(k) 
0  0  1 


(2.14) 


2.3.2  Feature  model 

A  feature  is  a  discrete,  fixed,  and  identifiable  landmark  of  the  environment  that  can 
be  consistently  and  reliably  observed  by  the  vehicle’s  sensors.  Features  can  have  many 
physical  forms  and  can  be  both  active  features  (artificial  acoustic  beacons)  or  passive 
features  (points,  planes,  corners,  poles). 

Features  are  represented  mathematically  as  a  vector  of  parameters  defining  the 
landmark’s  properties.  This  thesis  focuses  on  the  simplest  of  all  feature  models,  the 
stationary  point  landmark.  The  point  feature  is  static  and  may  be  defined  by  two 
parameters  specifying  its  position  with  respect  to  a  global,  2-Dimensional  coordinate 
frame.  This  type  of  feature  is  observable  from  any  angle  at  any  distance. 

The  z*'’  point  feature  in  the  environment  is  represented  by  the  state  estimate 
parameters  defined  as 


H  = 


(2.15) 


Since  features  are  assumed  to  be  stationary,  there  is  no  additive  uncertainty  term 
due  to  movement  in  the  feature  model.  Thus,  a  trivial  relationship  exists  between 
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the  point  feature  state  at  time  A;  +  1  and  k.  Therefore,  the  point  feature  model  can 
be  represented  by 

x/,(A:  +  1)  =  x/.(A:)  =  x/,.  (2.16) 


2.3.3  Observation  model 

The  general  form  for  the  observation  model  for  the  feature  is  given  by 


Zi{k)  =  hi[x„(fc),X/(A:),  A;]  +  Wj(A:).  (2.17) 

The  vector  Zi{k)  is  the  observation  vector  at  time  k.  In  this  thesis,  the  observation 
vector  consists  of  range  and  bearing  measurements  taken  at  time  k.  The  range  (A:) 
and  bearing  z^^{k)  measurements  are  of  the  feature,  with  state  x/.(A:)  relative  to 
the  robot’s  location  Xv(A;).  The  measurement  vector  is  given  by 


(2.18) 


The  function  h,  is  the  observation  model.  The  observation  model  relates  the 
output  of  the  sensor  z;  to  the  state  vector  x  when  observing  the  landmark. 

The  random  vector  Wj  is  the  observation  noise.  All  unmodelled  sensor  character¬ 
istics  and  noise  corruption  are  represented  in  Wi{k).  This  observation  error  vector  is 
again  assumed  to  be  a  stationary,  temporally  uncorrelated  zero  mean  random  process. 
Because  of  this  assumption  and  the  defined  expectation  of  Equation  2.19,  [8,43] 


E[wi]  =  0,  VA; 


(2.19) 


Ri(A:)  if  z  =  j  =  A: 
0  otherwise 


(2.20) 


where  Ri(A:)  is  the  observation  error  covariance  matrix  of  Wj  at  time  k  such  that 
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R 


^rw 

0  Z 


0 

Vw 


(2.21) 


and  Zrw  and  are  the  variances  associated  with  the  noise  in  range  and  bearing, 
respectively. 

The  observation  model  used  in  this  thesis  is  given  by  Equation  2.22 


Zi{k  +  1)  =  hj[x(A;)]  +  Wi{k) 


(2.22) 


=  h4x„(A:),x/^(A;)]  +  Wi(A:),  (2.23) 


where  the  defined  observation  matrix  can  be  written  and  expanded  as 


hi[xy{k),xf^{k)]  = 


^{xf,{k)  -  x,{k)P  +  -  y,{k)r 


arctan 


y/.(fc)-vdfc) 


Or, 


(2.24) 


2.4  Stochastic  mapping  estimation  process 


Stochastic  mapping  (SM)  is  an  EKF  approach  to  the  CML  problem.  This  approach 
provides  a  recursive  method  for  a  mobile  robot  to  yield  bounded  estimates  of  vehicle 
and  landmark  locations  based  on  the  information  it  obtained  about  the  environ- 
ment  [29]. 

The  CML  problem  addresses  the  idea  of  a  vehicle  operating  in  an  unknown  envi¬ 
ronment  starting  at  an  unknown  location.  The  number  of  states  needed  to  estimate 
the  map  cannot  be  fixed  at  the  start  of  the  mission  because  the  number  of  features 
in  the  environment  will  not  be  previously  known.  Hence,  the  size  of  the  state  vector 
can  not  be  pre-determined  and  must  be  changed  during  the  estimation  process. 

Stochastic  mapping  considers  CML  as  a  variable-dimension  state  estimation  prob¬ 
lem,  A  single  state  vector  is  used  to  represent  the  map;  the  state  vector  contains 
estimates  of  vehicle  location  and  environmental  features.  The  state  size  increases  or 


2.4  STOCHASTIC  MAPPING  ESTIMATION  PROCESS 


37 


1 

J 

i 

1 

i 

□ 

Prediction  1 

Vehicle 

Landmark 

Stage  \ 

Model 

L 

Model 

x(k+1lk).P(k+1lk) 


Observation 

Model 


Observation 


z(k+1lk) 


z(k+1) 


v(k+1lk) 


Update 


x(k+1lk+1). 

P(k+1lk+1) 


Figure  2-4:  The  role  of  the  EKF  in  stochastic  mapping. 


decreases  as  features  are  added  or  removed  from  the  map.  An  associated  covariance 
matrix  contains  the  uncertainties  of  the  estimates  and  the  correlations  between  the 
vehicle  and  feature  estimates. 

Stochastic  mapping  builds  and  updates  a  feature  map  of  the  environment  from 
sensor  measurements.  Localization  is  performed  simultaneously  by  updating  the  ve¬ 
hicle’s  position  through  an  EKF.  The  approach  is  described  in  Figure  2-4. 

Section  2.2  defined  the  state  vector  and  associated  covariance  matrix  used  in  rep¬ 
resenting  the  map  in  the  SM  approach  to  CML.  The  system  models  employed  by  SM 
were  defined  in  Section  2.3.  Sections  2.4.1  and  2.4.3  present  the  estimation  process 
of  SM.  The  data  association  problem  is  reviewed  in  Section  2.4.4  and  Section  2.4.5 
addresses  how  the  representation  of  the  map  is  augmented  with  the  addition  of  new 
features. 


2.4.1  Prediction 

The  prediction  stage  of  the  stochastic  mapping  algorithm  uses  the  vehicle  model 
described  in  Equation  2.12  and  feature  model  described  in  Equation  2.16  to  generate 
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a  prediction  of  the  state  vector.  This  prediction  is  given  according  to 


x{k  +  l\k)  =  iy{k)x{k\k)  +  u„(A:) 

Xv{k  +  1|A:) 

Xf{k  +  1|A:) 

_  Uik)x{k)  +  Uy{k) 

Xf{k) 


(2.25) 


The  state  prediction  Equation  2.26  is  for  a  linear  system,  specifically  a  linear 
vehicle  model.  Our  thesis  utilizes  a  non-linear  model  as  described  in  Section  2.3.1. 
Thus,  using  the  linearized  vehicle  model,  Vf„,  in  Equation  2.14,  the  state  prediction  ' 
is  calculated 


x{k  -f  1|A:)  = 


Vfy{k)x{k)  +  Uy{k) 

Xf{k) 


(2.26) 


During  the  prediction  stage,  the  feature  state  estimates  remain  unchanged  because 
the  features  are  assumed  to  be  stationary. 

Although  features  are  assumed  to  be  stationary,  the  vehicle  is  in  motion  and  has 
an  associated  noise  vector  v„.  This  process  noise  is  assumed  to  be  uncorrelated  and 
zero  mean  and  can  therefore  be  represented,  based  on  Equations  2.9  and  2.10,  with 
covariance  Q. 

The  prediction  stage  of  the  filter  must  also  propagate  the  covariance  matrix 
through  the  vehicle  model.  Defining  Vf„  as  the  Jacobian  of  f„  evaluated  at  the 
estimated  vehicle  state  Xy,  the  prediction  of  the  covariance  matrix  is  described  by 


P{k  +  1|A:)  =  Vf„(A:)P(A:|^")Vf/(fc)  +  VUk)u{k)VfJ{k)  +  Q(fc),  (2.27) 

where  Vf„  is  the  Jacobian  of  f„  evaluated  at  the  estimated  vehicle  state  x„  defined 
in  Equation  2.14,  and  Vfu  is  the  Jacobian  of  f„  evaluated  with  respect  to  the  control 
input  u.  The  Jacobian  Vf„  can  be  written  and  expanded  as 
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AT  cos  6  0 

Vf„=  AT  sin  0  0  ,  (2.28) 

ATtmip/ L  ATvsec^  ip/L 

where  L  is  the  length  of  the  vehicle.  We  will  define  the  length  of  the  vehicle  to  be  1 
meter.  The  prediction  stage  of  stochastic  mapping  may  also  be  written 

P™  P,f  _  Vf,(fc)Pw{A;|*)Vt/(A:)  +  Vf,(fc)u(fc)Vf/((:)  +  q(fc)  Vf„(A:)P.Xtl*:) 

P/  Pff  .  (VUk)P.,(k\k)f  Pg{kK 

(2.29) 

The  prediction  of  the  estimated  state  and  associated  covariance  matrix  is  made 
at  each  time  step  regardless  of  whether  sensor  measurements  are  taken.  In  the  ab¬ 
sence  of  a  measurement,  the  prediction  estimates  the  position  of  the  mobile  robot  by 
propagating  the  state  (essentially  performing  dead  reckoning). 

2.4.2  Observation 

The  fusion  of  measurements  into  state  estimates  begins  by  calculating  a  predicted 
observation  at  time  k,  termed  Zi{k).  Applying  the  observation  model  Equation  2.22, 
a  predicted  measurement  is  calculated  by 

z{k  +  1|A:)  =  h[x„(A:+  l|fc),x/(A: -t-  l\k)],  (2.30) 

where  h[x„(A;  -1-  l\k),Kf{k  +  1|A;)]  is  defined  by  Equation  2.24. 

Observations  are  received  from  sensors.  These  observation  measurements  must  be 
associated  with  particular  features  in  the  environment.  An  association  between  the 
actual  measurements  with  the  predicted  measurements  is  generated.  The  innovation, 
u,  is  defined  as  the  difference  between  the  actual  observations,  z,  received  from  the 
systems  sensors,  and  the  predicted  observations,  z: 


u{k)  =  z{k)  -  z{k  -t-  l|fc). 


(2.31) 
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The  innovation  covariance,  S{k)  can  then  be  calculated  at  time  k.  It  is  computed 
from  the  current  estimate  of  the  covariance  matrix  P{k  +  1|A;),  the  covariance  of 
observation  noise  R(A:),  and  the  Jacobian  of  the  observation  model  h  evaluated  with 
respect  to  the  estimated  state  vector,  Vh^.  The  innovation  covariance  is  then 

S{k  +  1)  =  Vh,{k)P{k  +  l|fc)Vh/(fc)  +  R{k  +  1),  (2.32) 

The  full  measurement  Jacobian  Vhj,  is  a  combination  the  Jacobians  of  the  obser¬ 
vation  model  evaluated  with  respect  to  the  vehicle  and  feature  states.  This  is  noted 
because  each  observation  is  only  a  function  of  the  feature  being  observed.  Therefore, 
defining  Vh^  and  Vhy;  as  the  Jacobians  of  the  observation  model  with  respect  to 
vehicle  state  and  feature  state  z,  respectively,  the  overall  observation  Jacobian  can  be 
shown  in  the  form 


VK{k)  =  [VK{k)  0  ...  0  Vhjiik)  0  ...  0].  (2.33) 

For  the  purposes  of  this  thesis,  the  Jacobian  Vh^  is 


Vh„  = 


and  the  Jacobian  Vhy;  is 


Vv-y/i  Q 
r  r  ^ 

y^-Vv 


Vhyj  = 


y/j-Vv 
r  T 

yfi~yv  x/.~xv 
r2  r2 


where  r  is  the  range  defined,  r  =  ^(x/XA:)  -  x^[k)y^  +  {yf^{k)  -  Vv{k)Y. 


(2.34) 


(2.35) 


2.4.3  Update 

The  observation  Zi{k  +  1)  is  used  to  update  the  state  and  covariance  predictions 
yielding  new  estimates  at  time  A:-b  1.  The  new  state  update  x(A:  -I-  1|A:  -t-  1)  and 
updated  state  covariance  P(A;  -I-  1|A:  -f  1)  are 
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'k{k  +  l|fc  +  1) 
P(A;+l|fc  +  l) 


x(A;+l|A;)+W(A:  +  l)i/(A;+l)  (2.36) 

P(A:  +  l|;fc)  -  W{k  +  l)Sik  +  1)W^(A:  +  1),  (2.37) 


where  the  gain  matrix  is  given  by 


W{k  +  1)  =  P(A:  +  l\k)Vh/{k  +  l)S-\k  +  1).  (2.38) 

The  pose  estimate  and  associated  errors  are  updated  via  the  weighting  factor 
of  the  gain  matrix.  The  weighting  factor  is  proportional  to  P(A:  +  1|A:)  and  inversely 
proportional  to  the  innovation  covariance  S(fc+ 1).  Thus,  if  the  innovation  covariance 
is  large  compared  to  the  state  covariance,  the  weighting  factor  approaches  zero  and 
the  measurement  has  little  affect  on  updating  the  state  estimate.  Conversely,  if  the 
prior  state  covariance  is  large  compared  to  the  innovation  covariance,  the  weighting 
factor  approaches  identity  and  the  state  is  updated  taking  into  account  nearly  all  of 
the  difference  between  the  measurement  and  expected  value  [43,46]. 


2.4.4  Data  association 

The  data  association  problem  consists  of  relating  observations  and  measurements  to 
corresponding  features  included  in  the  map.  Data  association  algorithms  are  moti¬ 
vated  by  the  desire  to  assign  measurements  to  the  features  from  which  they  originate. 
The  most  common  method  for  performing  data  association  are  nearest  neighbor  tech¬ 
niques  [21, 27, 30, 38].  This  thesis  applies  the  nearest  neighbor  gating  data  association 
algorithm. 

This  section  describes  the  technique  which  allows  an  observation,  z,  to  be  associ¬ 
ated  with  a  landmark,  x/..  It  relies  on  the  innovation,  z^',  and  innovation  covariance, 
S.  Given  an  observation  z(/c),  according  to  Equations  2.31  and  2.32,  the  Mahalanobis 
distance  is  defined  as  7  where 
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7  =  i/^(A:)S  Hk)u{k)  (2.39) 

and  is  tested 

7  —  'Ygate-  (2.40) 

The  quantity  7  has  a  probability  distribution  and  can  be  used  to  accept  or 
reject  a  particular  association  given  a  confidence  level,  or  “validation  gate,”  'ygate- 
For  a  system  with  2  degrees  of  freedom,  a  value  of  9.0  yields  the  region  of  minimum 
volume  that  contains  the  observation  with  a  probability  of  98.9%  [3].  It  is  this  'ygate 
that  the  Mahalanobis  distance  is  gated  against 

Igate  =  9.0  (2.41) 

7  <  9.0  (2.42) 

The  test  is  performed  for  all  known  features,  z  =  1 . . .  Nf,  (all  landmarks  previously 
mapped).  For  all  measurements  z  that  can  be  potentially  associated  with  feature 
i,  7z  is  calculated  and  tested  according  to  Equation  2.40.  The  validation  of  this 
equation  defines  where  a  measurement  is  expected  to  be  found.  If  some  7*  <  'ygate, 
the  obseivation  is  used  to  update  the  state  estimates.  However,  if  an  observation  does 
not  gate  with  any  existing  feature,  it  can  be  used  to  initialize  a  new  feature  into  the 
map.  The  next  section  describes  how  a  feature  is  initialized  and  augmented  into  the 
SM  state  space  representation. 

2.4.5  Feature  initialization 

The  stochastic  mapping  algorithm  allows  for  features  to  be  added  to  and  removed 
from  the  state  vector  and  covariance  matrix  as  time  progresses.  When  a  new  feature 
is  observed  by  measurement  z,  it  must  be  initialized  and  added  to  the  map;  as  new 
features  are  observed  the  state  space  is  augmented.  The  initial  estimate  of  the  new 
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feature  state  is 


=  s[Mk),z{k)],  (2.43) 

where  Nf  represents  the  number  of  known  features  up  to  the  current  time  step.  The 
feature  initialization  model  g  maps  the  current  vehicle  estimate  and  observation  to  a 
new  feature  estimate.  These  new  state  estimates  are  appended  to  the  estimated  state 
vector  as  a  new  feature  estimate  consistent  with  the  feature  model,  Equation  2.16. 
The  estimated  state  vector  is  augmented  to  become 


x{k  +  1|A:  +  1) 


x(/j  +  1|A:) 


^fNf+l 


(2.44) 


The  uncertainty  of  the  new  feature  estimates  must  also  be  initialized  in  the  covari¬ 
ance  matrix  based  on  the  new  observation.  This  observation  was  taken  relative  to  the 
robot.  Clearly,  the  uncertainty  in  the  new  feature’s  estimated  location  is  correlated 
with  the  uncertainty  in  the  robots’s  position.  This  uncertainty  is  therefore  not  only 
correlated  with  the  vehicle,  but  also  correlated  with  the  other  map  state  estimates 
(features). 

In  order  to  account  for  the  correlation  of  the  estimated  new  feature  and  the  pre¬ 
viously  mapped  state  estimates,  the  covariance  matrix  must  be  augmented.  The 
augmented  covariance  matrix  is 


P(A:  +  1|A;  +  1) 


P(A:  + 1|A;) 

B^' 

B 

A 

(2.45) 


where  P(A:-fl|/i:)  is  the  estimate  in  the  covariance  matrix  obtained  from  the  prediction 
and  B  and  A  are  defined  as 
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B  =  Vg:,P(A:  + l|/c),  (2.46) 

A  =  Vg,P(A:  +  l|A:)Vg/  +  Vg,RVg/,  (2.47) 

and  R  is  the  covariance  error  in  observations  defined  in  Equation  2.20.  The  symbols 
Vgi  and  Vgj  are  the  Jacobians  of  the  feature  initialization  model  g  with  respect  to 
the  state  and  observation,  respectively.  The  feature  initialization  model  g  can  be 
written  as 


X/ 

Jncw 

Xy  -1-  ZrCOs(0  -f  Ztp) 

y„-|-  ZrSin(0  -1-  z^) 

Thus  Vgi  and  Vg^  can  be  computed  as 


(2.48) 


Vg.  = 


Vg.  = 


1  0  -ZrSm{6  +  z^) 

0  1  ZrCOS{0  +  Z^) 

cos(i9  +  z,^)  -ZrSin(0  +  z,^) 
sin{6-\-z^)  ZrCOs(^  +  Z,^) 


(2.49) 

(2.50) 


This  fiamework  allows  for  a  variable-dimension  state  representation  of  the  vehicle 
and  environment  while  still  providing  a  recursive  solution  to  the  navigation  problem. 


2.5  Structure  of  the  CML  problem 

This  section  discusses  conclusions  of  work  performed  by  Newman  [38]  and  presented  in 
Dissanayake  et  al.  [17]  that  describe  the  convergence  properties  of  the  environmental 
map  and  its  steady  state  behavior.  These  results  provide  crucial  insight  into  the  CML 
problem.  The  three  convergence  theorems  are: 

First  Convergence  Theorem:  The  determinant  of  any  sub-matrix  of  the  map 
covariance  matrix  decreases  monotonically  as  successive  observations  are  made. 
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Figure  2-5:  The  Stochastic  Mapping  Algorithm 


Second  Convergence  Theorem:  In  the  limit  the  landmark  estimates  become 
fully  correlated. 

Third  Convergence  Theorem:  In  the  limit,  the  lower  bound  on  the  covariance 
matrix  associated  with  any  single  landmark  estimate  is  determined  only  by  the  initial 
covariance  in  the  vehicle  estimate  Pov  ^.t  the  time  of  the  first  sighting  of  the  first 
landmark. 

The  first  theorem  provides  the  following  information.  The  uncertainty  of  a  state 
estimate  can  be  measured  quantitatively  by  taking  the  determinant  of  a  state  covari¬ 
ance  sub-matrix.  The  determinant  of  any  sub-matrix  of  the  map  is  a  measure  of  the 
volume  of  uncertainty  associated  with  the  respective  state  estimate.  This  theorem 
states  that  the  total  uncertainty  of  the  state  estimate  does  not  increase  during  the 
update  step  of  the  stochastic  mapping  algorithm;  error  is  added  during  the  predic¬ 
tion  step  and  subtracted  during  the  update  step  (refer  to  Sections  2.4.1  and  2.4.3). 
Thus,  the  error  estimates  of  absolute  location  of  features  diminishes  as  successive 
measurements  are  made. 

The  second  theorem  proves  that  error  estimates  for  the  vehicle  and  feature  states 
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decrease  as  more  and  more  observations  are  made.  As  the  number  of  observations 
approaches  infinity,  features  become  more  correlated.  Thus,  given  the  exact  location 
of  one  landmark,  the  location  of  all  other  landmarks  can  be  deduced  with  absolute 
certainty.  Therefore,  the  map  becomes  fully  correlated. 

The  third  theorem  states  that  the  absolute  error  for  a  single  feature  or  vehicle 
can  never  be  lower  than  the  absolute  vehicle  error  that  was  present  at  the  time  the 
feature  was  first  initialized  into  the  map.  This  proves  that  the  error  for  a  feature  not 
be  less  than  the  error  of  the  vehicle  that  was  present  at  the  time  when  the  feature 
was  first  observed. 

2.6  Summary 

This  chapter  presented  the  stochastic  mapping  algorithm  which  is  described  in  Fig¬ 
ure  2-5.  This  algorithm  serves  as  the  mathematical  framework  for  this  feature-based 
approach  to  CML.  It  is  the  estimation  algorithm  that  will  be  employed  in  this  thesis. 
A  brief  overview  of  the  work-horse  of  the  SM  algorithm,  the  EKF,  was  presented. 
The  steps  involved  in  SM  were  then  presented  as  well  as  methods  in  which  to  account 
for  data  association  and  initializing  features  into  the  map  representation. 


Chapter  3 


Planned  Perception 

This  chapter  contains  the  theoretical  contribution  of  this  thesis.  It  presents  a  tech¬ 
nique  for  adaptive  concurrent  mapping  and  localization  (CML)  based  on  motivating 
changes  in  robot  behavior  in  an  attempt  to  improve  CML  performance.  This  tech¬ 
nique  will  be  evaluated  by  integrating  planned  perception  with  CML. 

3.1  Introduction 

Described  in  Chapter  2,  SM  is  a  viable  solution  to  the  problem  of  operating  in  an 
unknown  location  in  an  unknown  environment.  CML  requires  a  mobile  robot  to  take 
observations  of  objects  in  its  environment.  It  maps  the  surrounding  features  based 
on  these  measurements  and  then  uses  this  map  for  navigation. 

Planned  perception  is  the  process  of  adaptively  determining  the  sensing  strategy 
of  the  mobile  robot.  The  goal  of  integrating  smarter  sensing  strategies  within  CML  is 
to  provide  the  mobile  robot  with  a  means  to  determine  the  optimal  action  given  the 
current  estimates  of  the  vehicle  and  map.  Planned  perception  involves  the  focusing 
of  sensory  efforts  to  selected  areas  of  importance.  Areas  of  importance  are  defined  as 
those  areas  and  features  which  will  improve  the  current  navigation  estimates  and/ or 
those  which  may  contain  new,  useful  features.  By  integrating  CML  with  planned 
perception  we  will  attempt  to  answer  the  question  of  how  a  mobile  robot  should 
behave  so  as  to  attempt  to  maximize  CML  performance. 
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Accurate  information  regarding  the  pose  of  the  vehicle  is  vital  for  mission  success. 
This  drives  the  basis  for  our  implementation  of  adaptive  CML;  planned  perception 
is  formulated  so  as  to  maximize  vehicle  certainty.  The  SM  algorithm  provides  an 
inherent  coupling  (through  cross-correlations)  of  the  estimated  states  and  associated 
uncertainties.  Maintain  of  these  cross-correlations  is  important  to  the  CML  pro¬ 
cess  [17, 38]  as  seen  by  the  theorems  described  in  Section  2.5. 

The  CML  process  is  a  map-building  system.  What  is  important  in  a  map-building 
system  is  maintaining  the  consistency  and  integrity  of  the  whole  map  and  the  estimate 
of  the  mobile  robot’s  position  within  it  [15]. 

Since  we  aim  to  minimize  vehicle  uncertainty,  our  proposed  approach  chooses  a 
landmark,  from  the  available  set  of  featui'es,  whose  re-observation  will  best  improve 
the  robot’s  estimated  state.  This  choice  is  based  on  criteria  that  will  be  described  in 
Section  3.4. 

Section  3.2  presents  scenarios  that  demonstrate  why  planned  perception  in  CML 
is  of  interest.  Section  3.3  reviews  previous  research  in  the  area  of  adaptive  sensing 
strategies.  The  proposed  algorithm  for  integrating  planned  perception  and  CML  is 
presented  in  Section  3.4.  A  summary  of  the  chapter  is  provided  in  Section  3.5. 

3.2  Motivation 

Why  is  it  important  to  adaptively  choose  a  sensing  and  motion  strategy  for  a  mobile 
vehicle?  Motivation  behind  integrating  planned  perception  and  CML  can  be  seen 
through  the  following  examples.  These  examples  give  an  insight  into  the  possible 
applications  and  motivating  scenarios  behind  planned  perception. 

Efficiency 

Planned  perception  and  adaptive  sensing  techniques  can  be  used  to  limit  sensing  to 
selected  regions  of  interest,  benefitting  navigation  precision  and  dramatically  reducing 
computational  requirements  [21].  Planned  perception  within  the  CML  algorithm  will 
benefit  mobile  robots  because  it  will  reduce  location  estimation  errors  and  reduce  the 
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time  and  energy  required  to  achieve  a  desired  location  accuracy.  Thus,  we  will  be 
improving  the  efficiency  of  the  CML  process  for  mobile  robots  [20]. 

Echo-location 

The  behavior  of  dolphins  and  bats  show  how  adaptive  sensing  can  be  beneficial.  These 
animals  use  sonar  systems  from  which  they  can  establish  and  maintain  “contact”  by 
receiving  echoes  from  features  within  the  environment  [20].  Bats  and  dolphins  are 
constantly  adaptive  their  movements  during  their  “flight  paths.”  This  allows  them 
to  get  a  better  understanding  of  the  environment  by  obtaining  sonar  responses  from 
multiple  vantage  points  [1]. 

This  process  is  similar  to  the  way  a  human  perceives  the  world.  One’s  eyes  are 
constantly  moving,  constantly  adjusting,  and  constantly  focusing  on  different  objects 
to  obtain  a  “picture”  of  the  world.  Thus,  observing  the  world  through  planned 
perception  is  inherent  in  both  humans  and  bats  and  dolphins;  humans  use  vision  and 
dolphins  and  bats  use  sonar. 

Military  applications 

There  is  great  potential  for  military  application  of  autonomous  underwater  vehicles 
(AUVs).  Examples  of  importance  to  the  military,  especially  the  U.  S.  Navy,  are 
those  associated  with  specific  aspects  of  Naval  Doctrine:  Command,  Control,  and 
Surveillance,  Power  Projection,  Force  Sustainment,  and  Battlespace  Dominance  [36]. 
Command,  Control,  and  Surveillance  is  defined  as  the  gathering,  processing,  and 
distribution  of  information  that  is  vital  to  military  operations.  Power  Projection  is 
the  ability  to  respond  to  any  call-to-duty  around  the  world  and  to  take  the  fight  to 
the  enemy.  Force  Sustainment  is  defined  as  the  capability  to  sustain  our  forces  at 
home  and  abroad  through  effective  operation  and  supply.  Battlespace  Dominance 
is  maintaining  superiority  of  the  always  shifting,  fluid,  zones  of  military  areas  of 
operation.  AUVs  can  provide  the  Navy  with  a  means  of  enhancing  the  above  missions 
through  their  inherent  stealth  and  by  reducing  the  risk  of  losing  human  life.  AUVs 
can  help  fulfill  the  needs  of  the  Navy  because  by  shaping  the  battlespace  in  areas 
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denied  to  traditional  maritime  forces. 

Unlike  current  manned  platforms,  AUVs  possess  the  capabilities  of  providing  au¬ 
tonomy  and  enhanced  deployability  while  keeping  men  out  of  harm’s  way.  These 
skills  are  desirable  in  any  maritime  operation.  AUVs  may  be  launched  from  multiple 
platforms  such  as  shore  facilities,  ships,  submarines,  and  aircrafts.  Autonomy  allows 
for  independent  operation  away  from  manned  forces.  This,  in  turn,  allows  for  manned 
platforms  to  perform  more  complex  tasks  while  extending  the  “reach”  of  their  forces. 

The  ability  of  an  AUV  to  perform  planned  perception  would  allow  the  vehicle  to 
strengthen  its  ability  to  navigate  reliably  while  accomplishing  its  mission.  Planned 
perception  allows  for  a  vehicle  to  focus  its  sensory  efforts,  focusing  on  features  of  good 
quality,  in  order  to  maintain  an  accurate  estimate  of  vehicle  state.  Thus,  planned 
perception  may  enable  a  vehicle  to  better  perform  missions  such  as  antisubmarine 
warfare  (ASW),  intelligence,  surveillance,  and  reconnaissance  (ISR),  and  undersea 
search  and  survey. 

ISR  missions  may  be  enhanced  by  planned  perception  aboard  AUVs.  Due  to 
their  smaller  size,  AUVs  provide  superior  maneuverability  and  are  able  to  operate  in 
shallow  waters  while  still  providing  the  ability  to  avoid  detection.  Thus,  AUVs  may 
be  used  for  shallow  water  reconnaissance.  Planned  perception  integrated  with  CML 
would  allow  a  vehicle  to  focus  on  features  of  good  quality.  This  would  be  beneficial 
in  MCM  (mine  counter  measure)  missions  because  it  provides  a  means  of  clandestine 
detection  and  mapping  of  a  mine  field. 

The  integration  of  CML  with  an  adaptive  sensing  strategy  would  also  enhance 
ASW  missions.  Planned  perception  would  allow  an  AUV  to  focus  its  sensory  efforts 
on  a  submarine  while  providing  a  means  to  determine  how  to  maneuver  to  acquire  the 
maximum  information  about  this  target.  AUVs  could  also  serve  as  an  autonomous 
weapons  platform.  Smarter  sensing  strategies  may  enable  the  vehicle  to  focus  on  its 
target  and  increase  the  probability  of  achieving  mission  success. 

One  of  the  greatest  fears  of  any  submariner  is  the  fate  suffered  by  those  lost  in 
submarines  such  as  the  USS  Scorpion,  the  USS  Thresher,  and  the  Russian  Submarine 
Kursk  [12, 40].  Currently,  the  ability  to  detect  submarines  on  the  ocean  floor  (a  subset 
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of  undersea  search  and  survey)  is  provided  through  either  manned  vehicles  or  the  use 
of  side-scan  sonar  deployed  on  towed  arrays  or  on  remotely  operated  vehicles  (ROVs). 
However,  AUVs  utilizing  planned  perception  may  provide  a  greater  advantage  to 
locating  objects,  such  as  submarines  and  shipwrecks,  on  the  bottom  of  the  ocean. 
Integrating  planned  perception  with  CML  would  enable  AUVs  to  detect  these  features 
and  classify  them  as  features  of  importance.  Therefore,  in  this  theater  of  operation, 
an  AUV  would  be  able  to  maneuver  itself  through  adaptive  sensing  strategies  to 
maximize  the  information  gained  from  the  environment. 

Closing  the  loop 

A  crucial  measure  of  success  is  the  frequency  with  which  the  vehicle  recognizes  its 
initial  location  after  it  closes  the  loop.  Closing-the-loop  defines  the  problem  for  CML 
as  it  is  the  most  important  measure  of  how  well  the  vehicle  understands  the  envi¬ 
ronment.  It  is  defined  as  the  ability  to  recognize  that  the  vehicle  is  back  in  the 
same  place  and  that  it  is  re-observing  a  feature  it  has  previously  sensed  and  mapped. 
Planned  perception  would  enable  the  vehicle  to  increase  its  chances  of  closing  the  loop 
by  relying  on  its  ability  to  maneuver  through  the  area  of  operation  using  adaptive 
sensing. 


3.3  Previous  Work 

Strategic  sensing  allows  for  a  mobile  robot  to  limit  its  area  of  sensing  to  those  of  inter¬ 
est.  This  section  reviews  work  related  to  the  focus  of  this  thesis:  the  demonstration  of 
how  the  CML  framework  could  be  improved  through  smarter  sensing  strategies  and 
how  a  mobile  vehicle  should  behave  so  as  to  attempt  to  optimize  CML  performance. 

Planned  perception  has  become  a  popular  research  topic.  The  idea  of  planned 
perception  has  been  investigated  in  the  area  of  robotics.  Research  synonymous  with 
planned  perception  include  active  perception  [47],  active  vision  [14, 15],  directed  sens¬ 
ing  [29],  adaptive  sensing  [20, 21],  adaptive  sampling  [6],  and  sensor  management  [32]. 
A  common  theme  among  the  various  adaptive  sensing  strategies  has  been  choosing 
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the  action  which  maximizes  the  information  gained  by  the  robot  after  evaluating  the 
different  sensing  actions  the  robot  can  perform.  While  implementing  this  concept  it  is 
imperative  to  develop  a  quantitative  method  of  evaluating  the  various  future  sensing 
actions  of  the  robot  [21], 

Bellingham  and  Willcox  [6]  and  Singh  [42]  have  applied  the  concept  of  strategic 
sensing  to  marine  robotics.  Singh  formulated  and  implemented  adaptive  sensing  on 
the  Autonomous  Benthic  Explorer.  Bellingham  and  Willcox  investigated  dynamic 
oceanographic  phenomena  for  AUVs.  Also,  Manyika  and  Durrant- Whyte  [32]  de¬ 
veloped  a  method  for  implementing  an  information  theoretic  approach  to  planned 
perception.  This  method  was  applied  to  a  mobile  robot  operating  indoors,  given  an 
a  priori  map,  and  sensing  using  multiple  scanning  sonars. 

Our  approach  is  motivated  by  Feder’s  work  [21]  and  is  closest  to  Davison’s  im¬ 
plementation  of  active  vision  [14,15].  Feder,  Leonard,  and  Smith  [20]  introduced  a 
metric  for  adaptive  sensing  defined  in  terms  of  Fisher  information.  The  goal  of  this 
approach  is  to  determine  the  action  that  maximizes  the  total  knowledge  about  the 
system  in  the  presence  of  uncertainties  in  navigation  and  observations.  The  next 
action  of  the  robot  is  chosen  to  attempt  to  maximize  the  robot’s  information  about 
its  location  and  all  the  features’  locations  (the  map)  [20].  The  action  that  maximizes 
the  information  is  expressed  as 


Ufc  =  argumax  Ifc+i|fe+i  =  argumin  Pfc+i|fc+i  (3.1) 

where  u  is  the  control  input,  I  is  the  Fisher  information,  and  P  is  the  error  covari¬ 
ance  [21].  A  cost  function  is  defined  as 


N  _ 

C(P)  =  tta/ det{Pyy)  +  TT  ^  yJdet{Pff)  (3.2) 

i=l 

The  action  to  take  is  given  by  evaluating  Equation  3.1  using  the  metric  in  Equa¬ 
tion  3.2  [21].  The  approach  was  demonstrated  via  simulation,  underwater  sonar 
experiments,  and  in-air  sonar  experiments  [20, 21] 

Davison  implemented  active  vision  on  a  mobile  robot  [14, 15].  His  work  provides  a 
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way  for  a  mobile  robot,  with  active  camera  sensors,  to  track  and  fixate  features  over  a 
wide  field  of  view.  Stationary  point  features  are  used  to  observe  and  obtain  estimates 
of  robot  state  utilizing  a  Kalman  filter  approach  [15].  Davison  and  Murray  propose 
three  questions,  or  issues,  that  need  to  be  considered  to  maintain  the  map  [14]; 

1.  Which  of  the  current  set  of  known  features  should  be  tracked? 

2.  Is  it  time  to  label  any  features  as  not  useful  and  abandon  them? 

3.  Is  it  time  to  look  for  new  features? 

The  third  question  is  addressed  when  less  than  two  features  are  currently  visible. 
Question  2  is  answered  based  on  a  method  that  abandons  features  if  more  than  half 
of  at  least  10  attempts  to  observe  that  feature  fail  when  it  should  be  visible. 

To  answer  the  first  question,  Davison  presents  a  way  [14, 15]  to  calculate  the 
volume  of  error  (termed  Vs)  in  3-Dimensional  space,  for  each  feature.  The  quantity 
Vs  is  based  on  a  metric  using  eigenvalues  and  is  evaluated  for  each  feature  currently 
visible.  The  error  with  the  highest  value  is  selected  to  be  measured.  The  goal  is 
to  observe  the  feature  with  the  greatest  uncertainty  in  order  to  “squash”  the  total 
uncertainty  [14]. 

3.4  Planned  Perception 

This  section  reviews  the  concept  of  planned  perception  and  derives  a  method  for 
integrating  planned  perception  and  concurrent  mapping  and  localization  within  the 
stochastic  mapping  algorithm. 

3.4.1  Concept 

SM  provides  a  means  to  perform  CML.  The  map’s  information,  consisting  of  the  robot 
and  features,  is  maintained  as  described  in  Chapter  2.  The  estimated  state  vector,  x, 
and  associated  covariance  matrix,  P,  are  defined 
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x(fc  +  1|A:)  = 


Xu(A:  +  l|fc) 
Xfi{k  +  1|A:) 

XfNj{k+  1|A:) 


(3.3) 


(3.4) 


The  sub- matrix  represents  the  uncertainty  in  the  vehicle  estimates.  Our  goal  is 

to  minimize  vehicle  uncertainty.  The  approach  relies  on  the  convergence  properties 
of  CML  reviewed  in  Section  2.5  and  proved  in  [17,38]. 


The  determinant  of  the  state  covariance  matrix,  Equation  3.4,  is  a  measure  of 
the  volume  of  the  uncertainty  ellipsoid  associated  with  the  state  estimate  [17].  Thus, 
the  determinant  of  the  sub-matrix  Pyy  represents  the  uncertainty  ellipsoid  associated 
with  the  vehicle  state  estimate  x„  .  It  is  this  determinant  that  we  will  attempt  to 
minimize  through  adaptively  determining  the  sensing  strategy  of  the  mobile  robot. 


A  mobile  robot  is  given  the  task  of  performing  a  specified  mission.  Performing 
this  mission  is  defined  as  exploration.  Exploration  is  the  process  of  attempting  to 
perform  the  mission  objective.  Mission  objectives  vary  from  scenario  to  scenario  but 
some  examples  are:  navigating  between  waypoints  [21],  traversing  the  corridors  of 
a  building  [15],  and  detecting  objects  on  the  seabed  [27].  These  are  only  a  few 
examples  of  mobile  robot  mission  objectives,  the  possible  missions  a  robot  may  be 
asked  to  perform  are  innumerable. 


Despite  the  specific  mission,  one  aspect  of  exploration  is  constant;  the  mobile  robot 
must  accurately  navigate  to  accomplish  the  objective.  Reliable  navigation  requires 
good  estimates  of  the  vehicle  state.  Our  proposed  approach  to  incorporating  planned 
perception  in  the  SM  algorithm  focuses  on  maximizing  vehicle  certainty,  therefore 
benefitting  navigation  precision. 
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While  a  vehicle  is  “confident”  in  its  estimates,  the  determinant  of  the  sub-matrix 
is  relatively  small.  Therefore,  the  mission  can  be  carried  out  as  planned.  However, 
when  the  uncertainty  in  vehicle  estimates  grows,  the  determinant  of  P „„  also  grows. 
Thus,  the  vehicle  becomes  less  certain  of  its  overall  state.  It  is  at  this  instant,  when  the 
determinant  of  Pyy  exceeds  some  threshold,  that  our  method  of  planned  perception 
motivates  changes  in  robot  pose  and  hence,  sensing  locale.  The  method  of  switching 
operating  mode  from  exploration  to  localization  (planned  perception)  is  a  function  of 
vehicle  uncertainty  and  can  be  shown  in  Figure  3-1. 

As  successive  observations  are  made,  landmark  estimates  become  fully  corre¬ 
lated  [17].  This  is  the  second  convergence  theorem  proved  by  Newman  in  his  the¬ 
sis  [38].  This  implies  that  features  become  progressively  more  correlated  as  successive 
observations  are  made.  This  theorem  also  implies  that  in  the  limit,  the  map  be¬ 
comes  fully  correlated  and  given  the  exact  location  of  any  one  landmark,  the  exact 
absolute  location  of  the  vehicle  or  any  other  feature  is  deduced  [22].  Thus,  as  suc¬ 
cessive  observations  are  made  the  error  associated  with  vehicle  estimates  decrease 
monotonically  [17].  These  theorems  motivate  our  approach. 

When  the  uncertainty  of  the  vehicle  estimates  grows,  re-observing  a  feature  al¬ 
ready  mapped  will  update  and  improve  the  state  estimates.  This  is  because  the  total 
uncertainty  of  the  state  estimate  does  not  increase  during  an  update  [17].  The  ques¬ 
tion  then  arises,  which  feature  should  the  vehicle  attempt  to  re-observe.  Our  method 
of  determining  the  feature  the  vehicle  heads  toward  is  defined  below. 

This  is  the  goal  of  our  algorithm;  in  order  to  minimize  vehicle  uncertainty,  planned 
perception  determines  which  feature  the  vehicle  should  steer  toward.  Re-observing 
features  will  diminish  vehicle  uncertainty.  Once  vehicle  uncertainty  decreases  and  the 
determinant  of  P^„  is  again  below  a  given  threshold,  the  vehicle  returns  to  exploration 
and  carries  out  its  mission. 

3.4.2  Criteria 

The  proposed  planned  perception  approach  considers  three  questions: 
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Mission  style 
(operation  mode) 


Figure  3-1:  The  Overall  Mission  Control  Loop 


1.  For  all  mapped  features  i  =  1, . . . ,  the  re-observation  of  which  feature  i  will 
best  improve  vehicle  estimates? 


2.  What  is  the  cost  of  re-observing  feature  i  ? 


3.  What  is  the  risk  of  re-observing  feature  i  ? 


The  first  question  relates  to  the  estimate  of  the  vehicle  state  and  associated  covari¬ 
ance  matrix.  The  second  question  presents  a  means  to  account  for  the  cost  of  moving 
toward  feature  i.  Battery  life  and  power  are  important  to  autonomous  robots.  There¬ 
fore,  moving  to  a  far  away  feature  will  require  more  energy  to  be  spent.  Feder  states: 
the  problem  of  associating  measurements  with  features  in  cluttered  environments  re¬ 
mains  a  challenging  and  important  problem  for  any  estimation  problem  that  faces 
data  association  ambiguities  [21].  The  third  question  address  this  problem  of  data 
association. 
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Question  1:  For  all  mapped  features  i  =  l,...,Nf,  the  re-observation  of 
which  feature  i  will  best  improve  vehicle  estimates? 

The  determinant  of  is  the  measure  of  the  volume  of  uncertainty  associated  with 
the  vehicle  state  estimate.  The  total  uncertainty  of  the  state  estimate  does  not 
increase  during  an  update.  An  observation  Zi(k  +  1)  is  used  to  update  the  state  and 
covariance  predictions  yielding  new  estimates  at  time  k+  I-  The  new  state  update 
St{k  +  1|A:  +  1)  and  updated  state  covariance  P(A:  +  l\k  +  1)  are  computed 


x(A;  +  l\k  +  1) 
P(A:  +  l|fc  +  l) 


it{k  +  l\k)+W{k  +  l)u{k  +  l),  (3.5) 

P{k  +  1|A:)  -  W(fc  +  1)S(A:  +  1)W^(A:  +  1),  (3.6) 


where  W  is  the  weighted  filter  gain  and  S  is  the  innovation  covariance  as  described 
in  Section  2.4.  Thus,  the  covariance  matrix  is  updated  based  on  the  matrix 
W(A:  +  1)S(A:  +  1)W^(A:  +  1).  The  matrix  WSW^  can  be  represented  as 


WSW^  = 


(WSW^)„„ 

(WSW^)„/ 

_  {WSW’-)J/ 

(WSW^)// 

(3.7) 


Since  our  approach  focuses  on  minimizing  vehicle  uncertainty,  we  will  concentrate 
on  the  sub-matrix  (WSW^)„„  which  will  be  referred  to  as  WSWj.  The  sub-matrix 
P„„  is  updated  by  the  sub-matrix  WSWj  through  the  calculation 


P^fc  -h  l\k  -I- 1)  =  P4A:  +  1|A:)  -  [W(A:  +  l)S(fc  -b  l)W(fc  -t- 1)^]„  (3.8) 

Thus,  since  we  desire  to  maximize  vehicle  certainty,  we  want  the  maximum  change 
WSWj’  can  provide.  Representing  the  update  Equation  3.8  in  terms  of  determinants 


1  P„(k  +  1|*;  +  1)  I  =  I  P„(J:  +  IIA)  I  -  I  WSWj  I 


(3.9) 
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it  is  clear  that  to  minimize  the  updated  covariance  of  the  vehicle,  we  need  to  maximize 

I  WSW^  |. 

While  performing  planned  perception,  the  vehicle  desires  re-observe  a  known  fea¬ 
ture.  Thus,  the  vehicle  simulates  a  measurement  to  every  known  feature  i.  The 
associated,  simulated  matrix  (WiSiWf)„  is  calculated  for  each  feature.  Because 
I  WSWJ  I  needs  to  be  maximized,  the  vehicle  chooses  to  steer  toward  feature  i 
based  on  the  metric 


argi  max\  (WiSiWf)„  |  (3.10) 

The  goal  is  to  steer  towards  feature  i  that,  if  observed,  would  most  reduce  P„„. 


Question  2:  What  is  the  cost  of  re-observing  feature  i  ? 

Energy  efficiency  is  a  motivator  for  choosing  actions  to  be  performed  by  autonomous 
robots.  This  question  address  the  expense  of  driving  the  vehicle  toward  a  relatively  far 
landmark.  The  cost  of  determining  to  steer  toward  a  distant  feature  is  proportional 
to  the  distance  between  feature  i  and  the  robot’s  current  position.  In  2-Dimensional 
space,  the  distance  (represented  j]  •  ||)  between  two  points  (pi  and  P2)  is  calculated 

IIPI  -  P2II  =  -  (P2x))2  +  ((Ply)  -  (P2y))2  (3.11) 

where  pi  =  [pi^  pi^]^  and  ps  =  [p2x  Pzy]^- 

The  distance  between  the  current  estimated  robot  position  and  feature  i,  ||xyi-Xi,||, 
can  be  calculated  according  to  Equation  3.11 

W^fi  -  Xvll  =  y/ (x/,  -  a:„)2  -|-  (y^,  -  y„)2  (3.12) 

This  distance  ||x^  —  x„||  serves  as  our  second  criteria  to  determining  the  feature 
the  vehicle  steers  toward. 
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Question  3:  What  is  the  risk  of  re-observing  feature  il 

This  question  is  a  one  of  data  association.  Cluttered  environments  pose  a  threat 
to  correctly  associating  measurements  with  features.  Our  approach  considers  the 
consequences  of  data  association  errors  present  in  cluttered  environments.  For  each 
estimated  feature  i  =  1, ...  ,Nf,  the  mean  number  of  features  within  a  certain  distance 
are  calculated.  This  distance  is  a  defined  threshold  (i.e.  two  meters)  and  is  used  to 
calculate  the  density  of  features  in  a  certain  area.  If  feature  density  is  relatively  large, 
a  measurement  may  be  associated  with  the  wrong  feature;  this  may  cause  divergence 
due  to  a  data  association  error  [21]. 

Thus,  our  third  and  final  criteria  for  determining  which  feature  the  vehicle  should 
steer  toward  is  a  function  of  the  density  of  features  surrounding  feature  i,  termed 
density fi.  The  mean  number  of  features  within  a  certain  distance  is  calculated  to 
serve  as  the  density  function. 

3.4.3  Algorithm 

As  described  in  Section  3.4.2,  there  are  three  aspects  which  govern  our  planned  percep¬ 
tion  algorithm  of  choosing  which  feature  to  steer  toward.  The  criteria  are  summarized 

•  Which  feature,  when  re-observed,  will  best  reduce  vehicle  uncertainty? 

•  What  is  the  cost  of  driving  toward  a  feature? 

•  What  is  the  risk  of  associating  a  measurement  of  that  feature  with  the  wrong 
feature? 

We  want  to  develop  a  quantitative  method  for  evaluating  which  feature  the  ve¬ 
hicle  should  head  toward.  Combining  these  three  criteria  into  one  equation  involves 
1  (WiSiWf)„  I,  11%  -  x„l|,  and  a  function  of  the  density  of  features  surrounding 
feature  i,  f  {density fi).  The  three  criteria  will  be  represented  as  a  scalar  number,  F, 
calculated 


Ti  =  -aj  (WiSiWf  )„  I  -h  /3||%  -  %||  -b  C  •  f  {density fi) 


(3.13) 
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where  a,  P,  and  C  are  weighted  gains  for  the  different  criteria,  a  represents  the  gain 
for  the  information  desired.  /?  represents  the  gain  for  the  distance  between  the  robot 
and  feature  i  C  is  the  gain  for  the  measure  of  the  density  of  landmarks  around  feature 
i  Our  goal  is  to  minimize  Fj  in  Equation  3.13. 

While  the  determinant  of  is  below  the  given  threshold,  the  vehicle  performs 
according  to  its  assigned  mission;  while  P,„,  is  relatively  small,  explore.  When  the 
vehicle  is  no  longer  “confident”  in  its  state  estimate,  the  planned  perception  algorithm 
acts  as  the  controller  and  determines  which  feature  to  steer  toward.  This  can  be  seen 
in  Figure  3-1. 

The  localization  mode,  that  of  adaptively  sensing  the  environment  through  planned 
perception,  is  based  on  Equation  3.13.  This  equation  is  computed  for  all  mapped 
features  by  simulating  a  measurement  to  each  feature.  Each  mapped  feature,  i  = 
1, . . . ,  Nf,  has  an  associated  Fj  value.  When  all  Fj  are  computed  for  one  iteration  of 
the  filter,  the  vehicle’s  control  inputs  are  changed  to  steer  toward  feature  i*  that  is 
associated  with  the  minimum  value  Aj*  defined 

Ai.  =  argi  min  Ti.  (3.14) 

Thus,  the  vehicle  chooses  to  steer  toward  the  feature  i*  associated  with  Ai*  in  Equa¬ 
tion  3.14. 

Performing  the  above  planned  perception  approach  will  cause  the  vehicle  to  pos¬ 
sibly  desire  to  steer  toward  a  different  feature  i*  at  each  time  step.  This  continuous 
changing  of  desired  features  may  cause  the  vehicle  to  oscillate  between  control  inputs. 
To  compensate  for  this  oscillation,  we  add  in  some  hysterisis. 

Hysterisis  is  a  retardation  of  the  effect  of  an  action.  Adding  some  hysterisis  into 
our  decision  process  accounts  for  the  possibility  that  our  algorithm  may  determine 
the  vehicle  should  head  toward  a  different  feature  n  at  each  iteration.  Therefore,  to 
ensure  that  our  vehicle  is  steering  toward  the  feature  which  bests  suits  our  criteria 
for  the  entire  time  it  is  in  localization  mode,  we  store  the  value  Aj*  at  each  time  step. 
This  provides  us  with  Ai,(A;). 
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Figure  3-2:  The  integration  of  Planned  Perception  in  Stochastic  Mapping 


At  each  time  step  k  during  the  localization  mode  of  mission,  an  associated  Au,{k) 
value  is  stored.  This  value  is  compared  to  —  1).  If 


AUk)  <  Ai.{k  -  1)  (3.15) 

the  new  feature  i*  {k)  associated  with  Ai*(A:)  is  the  fe'ature  the  vehicle  determines  to 
steer  toward.  However,  if  Ai^{k)  >  Aj*(fc  —  1),  the  vehicle  continues  to  steer  toward 
the  feature  associated  with  Ai*(A:  —  1). 

The  action  of  the  robot  is  given  by  evaluating  Equation  3.14  using  the  metric 
Equation  3.15.  The  vehicle  steers  towards  the  feature  which  satisfies  the  above  crite¬ 
ria. 

3.4.4  Planned  perception  summary 

Planned  perception  is  integrated  into  stochastic  mapping  as  shown  in  Figure  3-2  where 
the  planned  perception  step  is  derived  from  Figure  3-1.  The  algorithm  for  performing 
planned  perception  is  summarized  below. 
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1.  while  det(P„„)  <  det(P„„f/,res/tow)  do  — >  explore 

2.  control  steer  toward  current  waypoint 

3.  if  distance  to  current  waypoint  is  <  3  m,  control  steer  toward  next  waypoint 

4.  end  while 

5.  while  det(Pt,„)  >  det{P ^ythreshoid)  do  — >  planned  perception 

6.  V  features  i  =  1,. . .  ,Nf,  simulate  measurement 

7.  for  each  featurei  and  simulated  measurement,  calculate 

8.  Innovation  Covariance  S 

9.  Filter  Gain  W 

10.  WiSiWf 

11.  det(WiSiWf) 

12.  ||xy;-x„|| 

13.  mean  number  of  features  around  feature^  \ — >  density fi 

14.  Ti  =  -a  det(WiSiWf )  +  /3\\kfi  -  x„||  +  Cidensityfi) 

15.  Ai»(/c)  =  argi  min  Ti{k) 

16.  if  Ai^{k)  <  -  1)  I — >  store  At«(/c),  otherwise  Ai*(A:  -  1)  =  Ait{k) 

17.  Vehicle  heads  toward  feature  i*  — >  control  steer  to  feature  i* 

18.  end  while 


3.5  Summary 


This  chapter  presented  the  theory  and  algorithm  of  our  approach  to  performing 
planned  perception.  The  idea  is  based  on  analyzing  the  uncertainty  in  vehicle  esti¬ 
mates.  Planned  perception  determines  what  feature  the  vehicle  should  head  toward. 
The  presented  strategy  of  performing  adaptive  sensing  can  easily  be  incorporated  into 
the  stochastic  mapping  algorithm. 


Chapter  4 


Simulation  Design  and  Results 


This  chapter  is  concerned  with  the  simulation  design  and  results.  A  description  of 
the  simulator  is  presented  in  Section  4.1.  This  chapter  also  presents  the  results  from 
the  simulations  of  the  integrated  planned  perception  and  concurrent  mapping  and 
localization  (CML)  algorithm  described  in  Chapter  2  and  Chapter  3. 


4.1  Simulation  design 

This  section  describes  the  simulation  designed  to  implement  the  incorporation  of 
planned  perception  within  CML.  The  simulation  is  coded  in  (c)  MATLAB  and  is 
described  in  Figure  4-1.  Each  step  of  the  simulation  is  presented  below.  This  presen¬ 
tation  is  a  general  overview  of  the  simulation  based  on  the  algorithm  and  procedures 
set  forth  in  Chapter  2  and  Chapter  3.  For  a  more  in  depth  discussion  on  each  process 
in  Figure  4-1,  refer  to  these  Chapters. 

4.1.1  Setup  and  initialization 

This  step  of  the  simulation  process  is  performed  as  the  first  step  of  the  simulation. 
It  defines  and  initializes  all  variables  that  will  be  used  throughout  the  estimation 
process.  During  the  setup,  all  matrices,  vectors,  and  variables  are  defined  and  the 
specific  size  of  each  matrix  is  set.  The  estimated  state  vector,  x,  is  initialized  to 
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zero  with  an  initial  uncertainty.  Vehicle  uncertainty  is  initialized  with  a  standard 
deviation  of  0.1  m  in  both  the  x  and  y  direction,  and  a  heading  standard  deviation 
of  0.1  degree. 

The  setup  process  also  defines  the  parameters  of  the  system.  The  parameters  are 
defined  in  Table  4.1. 


Table  4.1:  System  parameters 


sampling  period,  AT, 

1  s 

vehicle  cruise  speed,  v 

.75  m/s 

max.  steer  angle  of  vehicle 

22.5° 

speed  process  noise  std.  dev. 

0.25  m/s 

heading  process  noise  std.  dev. 

1.0° 

range  of  sensor 

25  m 

sensor  field  of  view 

±90° 

probability  of  measurement  return 

90% 

sensor  range  noise  std.  dev. 

0.1  m/s 

sensor  bearing  noise  std.  dev. 

1° 

The  noise  inherent  in  the  process  and  observation  models  are  also  defined.  The 
noise  covariances,  Q  and  R,  are  associated  with  the  vehicle  and  observation  models, 
respectively.  The  covariances  are  discussed  in  Section  2.3.1  and  Section  2.3.3  and  are 
defined 


Q  = 

R  = 


0.52  0 
0  12 
0.12  0 
0  12 


(4.1) 

(4.2) 


The  parameter  and  noise  values  are  not  chosen  arbitrarily.  They  are  chosen  to 
reflect  actual  values  apparent  in  systems.  The  sensor  is  modelled  after  the  SICK  laser 
scanner  used  in  the  Marine  Robotics  Laboratory  at  the  Massachusetts  Institute  of 
Technology.  The  vehicle  is  modelled  to  have  two  scanners  to  provide  range  and  bearing 
measurements.  The  range,  field  of  view,  and  probability  to  return  a  measurement  are 
described  in  Table  4.1. 
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Figure  4-1:  The  Simulation  Design. 
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The  parameters  and  defined  matrices  are  passed  to  the  filter. 

4.1.2  Estimation  process 

This  section  describes  the  estimation  process  of  the  simulation.  The  estimation  pro¬ 
cess  consists  of  the  prediction,  data  association,  update,  and  feature  initialization 
steps  of  the  simulation. 


Prediction 

The  prediction  step  of  stochastic  mapping  described  in  Section  2.4.1.  It  consists  of 
predicting  the  true  vehicle  states,  the  estimated  vehicle  states,  and  the  covariance 
matrix. 

The  true  and  estimated  vehicle  states  are  predicted  by  propagating  the  previous 
states  through  the  vehicle  model.  The  states  of  the  vehicle  are  predicted  by 


Xy 

Xy  -1-  vAT  cos{6) 

Vv 

|/c-4“l 

Uy  +  vAT  sin(0) 

e 

6  +  ip 

(4.3) 


where  v  and  ip  are  speed  and  heading  control  inputs  with  the  process  noise  given  in 
Table  4.1.  The  control  inputs  are  given  from  the  control  input  step  of  the  simulation. 
This  process  will  be  described  in  Section  4.1.4. 

The  covariance  matrix  P  is  also  estimated  during  the  prediction  step.  To  esti¬ 
mate  P,  the  Jacobians  Vf^  and  Vf^  are  calculated  according  to  Equation  2.14  and 
Equation  2.28,  respectively.  The  prediction  of  the  covariance  matrix  is 


P(fc  +  1|A:)  =  Vf,{k)P{k\k)Vi/{k)  -F  Vf„(A:)u(fc)Vf/(A:)  +  Q{k).  (4.4) 


The  prediction  process  is  described  as 
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1.  do  — >  prediction 

2.  x(fc  +  1|A:  =  f„[x„(A:),  u„(fc  +  1)]  (truth  projection) 

3.  Xv{k  +  1)  =  f„[x„(A:),  u„(fc  +  1)]  (estimate  projection) 

4.  Q  <—  u{k)  (process  noise  covariance) 

5.  Vf„  ^  (Jacobian  of  w.r.t.  vehicle  state) 

6.  Vfu  (Jacobian  of  w.r.t.  control  input) 

7.  P(fc  +  l\k)  =  Vf„(A:)P(A:|A:)Vf/(A:)  +  Vf„(fc)u(fc)Vf/(fc)  +  Q(fc)  (projection) 

8.  end  prediction 


The  predicted  estimated  state  vector  and  covariance  matrix  are  used  to  attempt 
to  assign  features  to  measurements  received  from  the  sensors  in  the  data  association 
step  of  the  simulation. 


Data  association 

Data  association  is  relating  observations  and  measurements  to  their  corresponding 
features.  Data  association  algorithms  strive  to  properly  assign  measurements  to  the 
features  from  which  they  originate.  Nearest  neighbor  gating,  the  process  described 
in  Section  2.4.4,  is  the  data  association  method  simulated. 

Nearest  neighbor  gating  is  simulated  as 


1.  do  — >  data  association 

2.  for  V  features,  i=l ...  Nf,  calculate 

3.  Ui  =  Zi  —  Zi^  (innovation) 

4.  Vhi  (Jacobian  of  h  w.r.t.  state  estimate) 

5.  Si{k  +  1)  =  Vhi(A:)P(A:  +  l|/!:)Vha:^(A:)  +  R(fc  + 1)  (innovation  covariance) 

6.  7j  =  <  9  (association  test) 

7.  if  7i  <  9  — >  update 

8.  end  for 

9.  if  no  feature  gates,  — >  do  feature  initialization 
10.  end  data  association 


The  gating  is  performed  where  9  is  chosen  as  the  gating  parameter  based  on  a 
distribution  as  defined  in  Equation  2.40.  Also,  based  on  the  estimated  state  vector 
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and  observation  model  described  in  Section  2.3.3,  the  Jacobian  Vhj,  is  calculated 
according  to  Equation  2.33. 


Update 


The  update  step  of  simulation  is  straightforward;  this  step  updates  the  estimates  of 
the  state  vector  and  covariance  matrix  based  on  the  observation  z.  The  estimates  are 
updated,  as  described  in  Section  2.4.3,  according  to 


1.  do  — »  update 

2.  W(A:  +  1)  =  P(fc  +  l|fc)Vhi^(A:  + 1)S“^(A:  +  1)  ^  (weighted  gain  /  Kalman  gain) 

3.  x{k  +  1|A:  +  1)  =  x{k  +  l|/c)  +  W(/c  +  l)i'{k  4- 1)  <=>  (state  update) 

4.  P(fc  +  l|fc4-l)  =  P(A:  +  1|A:)  — W(/c  +  l)S(fc+l)W^(/c  +  l)  <=>  (covariance  update) 

5.  end  update 


The  updated  estimates  are  passed  to  the  control  input  step  to  determine  how  the 
vehicle  should  act.  The  choice  of  control  input  is  described  in  Section  4.1.4. 


Feature  initialization 


This  step  of  the  simulation  is  called  when  no  previously  mapped  feature  is  associated 
with  a  measurement.  Feature  initialization  augments  the  estimated  state  vector  and 
covariance  to  incorporate  the  new  feature’s  location  and  uncertainty.  The  augmenta¬ 
tion  is  simulated  as 


4.1  SIMULATION  DESIGN 


69 


1. 

do  feature  initialization 

2. 

k{k  +  IjA:  -t- 1)  = 

x(fc  -t-  l\k) 

(augmented  state  estimate) 

3. 

B  =  WgJ>{k  +  1|A;) 

4. 

A  =  VgxP(fc  +  llA:)Vg/  -b  Vg,RVg/ 

r 

P(fc  +  1|A:-|-1)  = 

’  P(A:-bl|A:) 

■ 

<^(  augmented  covariance  matrix) 

0. 

B 

A 

6. 

increase  number  of  features 

7. 

end  feature  initialization 

This  step  of  the  simulation  calculates  the  appropriate  Jacobians  to  initialize  the  fea¬ 
ture  as  described  in  Section  2.4.5. 


4.1.3  Mecisurements 

Measurements  are  simulated  as  ranges  and  bearings  to  features.  Ranges  and  bearings 
are  simulated  as 


1.  do  — >  measurement 

2.  for  V  features,  i—l...Nf 

3.  if  feature  i  is  in  sensor’s  field  of  view,  add  feature  position  to  temporary  set 

4.  from  this  set,  randomly  choose  1  feature<;=>(feature  observing) 

5.  generate  random  number  between  0  and  1 

6.  if  random  number  >  0.9— >  Z  =  [  ]  •o^(false  return) 


7. 


else,  Z  = 


yj{xf.{k)  -  Xy{k)Y  -t-  {yf^{k)  -  yv{k)f 
4.  y/;W-yv{fe)  n 

arctan  ^;Jk)-Z{k)  “ 


8.  if  no  feature  is  in  the  sensor’s  field  of  view,  Z  =  [  ]  <j=>(no  return) 

9.  end  for 

10.  end  measurement 


The  measurement  process  described  above  is  simulated  for  every  time  step. 
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4.1.4  Control  input 

This  step  of  the  simulation  determines  the  control  input  to  the  vehicle.  The  control 
input  is  a  function  of  the  uncertainty  in  the  vehicle,  defined  P„v  While  the  uncertainty 
is  less  than  the  threshold,  R wthTeahoid^  fhe  vehicle  moves  from  waypoint  to  waypoint. 
When  the  vehicle  is  within  3  meters  of  the  current  waypoint,  the  control  heading 
is  changed  to  steer  the  vehicle  to  the  next  waypoint.  While  the  uncertainty  of  the 
vehicle  is  greater  than  the  given  threshold,  the  vehicle  performs  planned  perception. 
The  planned  perception  algorithm  is  described  in  Chapter  3. 

The  process  of  switching  between  mission  styles,  exploration  and  planned  percep¬ 
tion,  is  simulated 


1.  while  det(P„„)  <  det{P yythreshoid)  do  — ►  explore 

2.  control  steer  toward  current  waypoint 

3.  if  distance  to  current  waypoint  is  <  3  m,  control  steer  toward  next  wa3^oint 

4.  end  while 

5.  while  det(P„„)  >  det(P yythreshoid)  do  — >  planned  perception 

6.  V  features  i=  1, . . . ,  Nf,  simulate  measurement 

7.  for  each  featursi  and  simulated  measurement,  calculate 

8.  Innovation  Covariance  S 

9.  Filter  Gain  W 

10.  WiSiWj 

11.  det(WiSiWf) 

12.  ||xy;  -x„|| 

13.  mean  number  of  features  around  feature^  i — >  density fi 

14.  Fi  =  —a  det(WiSiWj’)  -|-  PW'x.fi  -  x„||  -I-  ({density ft) 

15.  Ai»(A:)  =  argi  min  Ti{k) 

16.  if  Ai»(fc)  <  Ai*(fc  -  1)  I — >  store  Ai*{k),  otherwise  Ai«(A:  -  1)  =  At*(A:) 

17.  Vehicle  heads  toward  feature  i*  — >  control  steer  to  feature  i* 

18.  end  while 
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4.2  Simulation  Results 

In  this  simulation,  the  vehicle  is  to  survey  an  area  steering  toward  waypoints.  The 
vehicle  starts  at  position  (0,0).  The  waypoints  are  defined  to  provide  a  “lawnmower” 
pattern,  survey  of  the  environment.  The  locations  of  the  features  and  waypoints  are 
defined  during  the  setup  process  of  the  simulation  in  Figure  4-1.  The  features  are 
2-Dimensional  point  features  and  are  chosen  from  the  set  where  x  and  y  are  random, 
independent  variables  which  are  uniformly  distributed  over  the  area  [x:0— >120]  and 
[y:-20^135].  The  features,  and  waypoints  define  the  environment  and  are  shown  in 
the  top  of  Figure  4-2.  The  desired  path  of  the  vehicle  is  shown  in  the  bottom  of 
Figure  4-2. 
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Figure  4-2:  Top:  The  simulation  environment.  The  red  circles  are  the  feature  loca¬ 
tions.  The  black  squares  are  the  waypoints.  Bottom:  The  desired  path  of  the  vehicle 
navigating  between  waypoints  is  shown  by  the  thick  red  line.  The  black  arrows  de¬ 
pict  the  direction  the  vehicle  takes  going  from  (0,0)  to  the  waypoint  (105,120).  The 
vehicle  then  back-tracks  performing  the  lawnmower  survey. 
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Desired  Path  of  Vehicle 
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In  the  first  simulation,  the  vehicle  moves  between  the  waypoints  navigating  only 
by  dead  reckoning.  In  the  second  simulation,  the  vehicle  performs  CML  as  it  navi¬ 
gates  according  to  the  stochastic  mapping  algorithm  defined  in  Chapter  2.  The  third 
simulation  integrates  planned  perception  and  CML.  This  simulation  is  performed 
constraining  the  overall  volume  of  vehicle  uncertainty  (the  determinant  of  Fi¬ 
nally,  the  fourth  simulation  performs  planned  perception  constraining  the  vehicle 
uncertainty  in  the  x  and  y  directions.  This  simulation  constrains  a  subset  of  the 
determinant  P,,^;  it  constrains  the  determinant  of  which  is  the  area  of  vehicle 
uncertainty  associated  with  the  x  and  y  positions  estimates.  The  simulations  inte¬ 
grating  planned  perception  and  CML  are  broken  into  scenarios  representing  different 
strategies  for  planned  perception  navigation. 

The  global  parameters  for  each  simulation  are  defined  in  Table  4.1  and  Table  4.2. 


Table  4.2:  Simulation  parameters 


sampling  period,  AT, 

1  s 

duration  of  simulation 

2000  s 

number  of  waypoints 

12 

number  of  features 

30 

initial  vehicle  x  position  uncertainty  std.  dev 

0.1  m 

initial  vehicle  y  position  uncertainty  std.  dev 

0.1  m 

initial  vehicle  heading  uncertainty  std.  dev 

0.1  deg 

4.2.1  Dead  reckoning 

Navigating  by  dead  reckoning  means  that  the  estimation  is  calculated  only  by  the 
vehicle  model.  There  is  no  observation  of  features.  In  terms  of  the  Kalman  filter  based 
stochastic  mapping  algorithm,  dead  reckoning  is  navigating  based  on  the  prediction 
step  alone.  Dead  reckoning  is  the  best  estimate  of  the  vehicle  model  without  the 
addition  of  noise.  Thus,  since  the  control  input  is  known  for  the  desired  path,  the 
estimated  path  navigating  by  dead  reckoning  is  the  same  as  the  desired  path.  This 
can  be  seen  in  Figure  4-3. 

Figure  4-3  shows  the  trajectory  paths  of  the  true  vehicle  and  the  estimated  dead 
reckoning  path.  The  absolute  position  error  for  dead  reckoning  grows  with  time. 
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This  error  grows  without  bounds  as  a  result  of  the  process  noise.  The  errors  in 
vehicle  states  are  shown  in  Figure  4-4.  Finally,  the  variances  in  x  and  y  position  are 
shown  in  Figure  4-5.  The  variances  show  how  the  position  error  grows  unbounded. 

Dead  reckoning  is  navigating  by  projecting  the  vehicle  through  the  vehicle  model. 
There  are  no  observations  of  features  to  “reset”  the  vehicle  estimate.  Thus,  the  error 
grows  without  bounds. 


East  (m) 


Figure  4-3:  Dead  Reckoning.  True  vehicle  path  versus  estimated  vehicle  path.  The  red 
crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the  estimated  vehicle  path. 
While  performing  dead  reckoning,  the  estimated  vehicle  path  is  the  same  as  the  desired 
path.  The  true  vehicle  path  differs  because  of  the  presence  of  process  noise. 


Heading  Error  (rad) 
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Actual  Errors  with  1  sigma  Bounds 


Figure  4-4:  Dead  Reckoning.  The  errors  for  the  vehicle  states  when  navigating  by  dead 
reckoning  are  shown.  The  actual  errors  are  represented  by  the  red  dashed  line.  The  solid 
blue  lines  represent  dzlrr  error  bounds.  The  errors  grow  without  bounds. 


Variance  (m  ) 
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time  (s) 


Figure  4-5:  Dead  Reckoning.  The  variances  in  x  and  y  position  are  plotted.  The  red  dotted 
line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As  expected,  these 
variances  grow  without  bounds. 
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Determinant  of  Vehicle  Uncertainty 


Determinant  of  Vehicle  x,y  Uncertainty 


Figure  4-6:  Dead  Reckoning.  Top:  The  determinant  of  the  overall  vehicle  uncertainty, 
P^^,.  Bottom:  The  determinant  of  the  uncertainty  in  the  vehicle  x  and  y  position, 

p 

^  xy 
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4.2.2  Concurrent  mapping  and  localization 

This  simulation  navigates  using  the  stochastic  mapping  algorithm  defined  in  Chap¬ 
ter  2.  Unlike  dead  reckoning,  stochastic  mapping  utilizes  observations  of  objects  in 
the  environment;  stochastic  mapping  is  a  feature-based  approach  to  CML.  Observ¬ 
ing  features  is  used  to  obtain  reference  points  for  navigation,  these  features  serve  as 
localization  points  whose  re-observation  reduces  vehicle  uncertainty. 

The  estimated  path  and  true  path  of  the  vehicle  are  compared  in  Figure  4-7.  The 
CML  error  estimates  are  bounded  as  shown  in  Figure  4-8.  They  converge  to  lower 
limits  as  defined  by  the  CML  convergence  theorems  described  in  Section  2.5.  The 
vehicle  uncertainty  converges  to  the  initial  vehicle  uncertainty  as  successive  obser¬ 
vations  are  made.  Because  the  estimated  uncertainty  can  only  be  decreased  during 
an  update,  the  vehicle  uncertainty  can  only  be  as  good  as  the  uncertainty  that  the 
vehicle  possessed  at  the  time  it  first  observed  a  feature.  The  variances  in  the  vehicle 
estimate  converge  as  shown  in  Figure  4-9. 

The  volume  of  uncertainty  in  the  vehicle  can  be  represented  as  the  determinant  of 
the  covariance  matrix.  The  top  of  Figure  4-10  shows  the  determinant  of  the  vehicle 
uncertainty,  The  x  and  y  uncertainty  is  shown  in  the  bottom  of  Figure  4-10 
as  the  determinant  of  the  uncertainty  in  Pxy-  The  jumps  in  these  figures  represent 
the  vehicle  re-observing  a  feature  and  updating  its  estimate.  This  re-observation  of 
features  proves  to  lower  the  uncertainty  in  the  vehicle  estimate. 
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Position  in  X  Direction  (m) 


Figure  4-7:  Concurrent  Mapping  and  Localization.  True  vehicle  path  versus  estimated 
vehicle  path.  The  red  crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the 
estimated  vehicle  path.  The  minor  jumps  in  estimated  and  true  position  occur  when  the 
vehicle  obtains  an  update  and  improves  its  estimated  position. 
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Figure  4-9:  Concurrent  Mapping  and  Localization.  The  variances  in  x  and  y  position  are 
plotted.  The  red  dotted  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance 
in  y.  As  expected,  these  variances  converge  to  lower  limits. 
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Determinant  of  Vehicle  Uncertainty 


Close-up  of  Determinant 


Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 

Figure  4-10:  Concurrent  Mapping  and  Localization.  Top  Left:  The  determinant 
of  the  vehicle  uncertainty,  This  determinant  converges  to  the  determinant  of 
the  vehicle  uncertainty  at  the  time  the  first  feature  was  observed.  Top  Right:  The 
close-up  shows  the  convergence  of  the  uncertainty.  Bottom  Left:  The  determinant 
of  X  and  y  vehicle  uncertainty,  Pi,,.  Bottom  Right:  The  close-up  shows  that  in  the 
limit,  as  successive  observations  are  taken,  the  vehicle  uncertainty  converges  to  the 
initial  x  and  y  uncertainty  of  the  vehicle  at  the  time  it  first  observed  a  feature. 
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4.2.3  Planned  perception  simulation  #1 

This  simulation  integrates  planned  perception  and  CML.  The  vehicle  navigates  from 
waypoint  to  waypoint.  The  planned  perception  algorithm  goes  into  effect  when  the 
uncertainty  in  the  vehicle  estimate  reaches  a  certain  threshold.  This  simulation  is 
broken  down  into  scenarios  demonstrating  different  strategies  of  integrating  planned 
perception  and  CML. 

As  discussed  in  Section  3.4,  planned  perception  chooses  which  feature  the  vehicle 
should  steer  toward  defined  by  the  equation 

r,  =  -at  (WiSiWf  )„  I  +  /3||xyj  -  x„|l  +  C  •  f  {density fi).  (4.5) 

The  action  of  the  robot  is  given  by  evaluating  Equation  4.6  using  the  metric  Equa¬ 
tion  4.7.  The  vehicle  steers  towards  the  feature  which  satisfy 

Ai*  =  argi  min  Fj,  (4.6) 

Ai,{k)  <  AUk  -  1).  (4.7) 

The  first  scenario  addresses  only  the  first  criteria  for  planned  perception:  For  all 
mapped  features  i  =  1, .  ■  • ,  Nf,  the  re-observation  of  which  feature  i  will  best  improve 
vehicle  estimates?  The  second  scenario  addresses:  What  is  the  cost  of  re-observing 
feature  i  ?  The  third  scenario  combines  both  of  these  criteria  along  with  addressing 
the  data  association  problem. 

For  each  scenario,  the  uncertainty  determinant  threshold  is  set  at  Pwthreshoid  = 
0.05. 

Planned  perception  -  Scenario  1 

This  scenario  addresses  the  first  criteria  of  planned  perception.  It  only  address 
I  (WjSjWf)„  I  Equation  4.5.  Thus,  the  weighting  gains  (3  and  C  are  set  to  zero. 
The  cost  and  risk  of  associated  with  re-observing  feature  i  are  not  addressed  in  this 
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scenario. 

The  trajectories  of  the  true  and  estimated  vehicle  paths  are  shown  in  Figure  4-11. 
The  vehicle  deviates  from  the  desired  path,  Figure  4-2,  when  the  uncertainty  for  the 
vehicle  exceeds  the  given  threshold.  When  this  occurs,  the  vehicle  chooses  to  steer 
toward  feature  i  that  satisfies 


Ti  =  -a\  {WiSiWj),  I,  (4.8) 

where  a  is  set  to  10^^  to  accurately  compare  the  determinants  of  (WjSiWf  )t,.  This 
gain  was  set  after  analyzing  the  determinant  values  associated  with  Equation  4.5. 

By  deviating  from  the  desired  path,  the  vehicle  is  able  to  re-observe  features  that 
would  otherwise  not  have  been  in  the  sensor’s  field  of  view.  This  allows  for  the  vehicle 
to  decrease  its  uncertainty  in  estimated  states  through  the  re-observation  of  features. 
The  re-observation  of  features  with  a  relatively  small  position  uncertainty  clearly 
provides  more  benefit  than  observing  features  with  relatively  larger  uncertainties  in 
position. 

Figure  4-14  represents  when  the  uncertainty  threshold  is  breached.  The  thresh¬ 
old  is  plotted  with  the  current  uncertainty.  The  feature  the  vehicle  steers  toward  is 
reflected  in  Figure  4-15.  The  vehicle  tends  to  steer  toward  features  that  are  mapped 
earlier  during  the  mission.  This  is  expected  because  these  features  have  less  uncer¬ 
tainty  in  their  position  estimates  and  provide  better  localization  points  for  vehicle 
state  estimation. 

These  results  are  consistent  with  the  convergence  theorems  described  in  Sec¬ 
tion  2.5.  The  first  convergence  theorem  states:  The  determinant  of  any  sub-matrix 
of  the  map  covariance  matrix  decreases  monotonically  as  successive  observations  are 
made.  This  means  that  the  error  in  the  estimates  of  the  absolute  location  of  the  ve¬ 
hicle  and  features  diminishes  as  successive  measurements  are  made.  This  is  the  point 
of  planned  perception;  when  the  vehicle  uncertainty  reaches  a  certain  threshold,  the 
vehicle  chooses  to  re-observe  certain  features  in  order  to  reduce  the  uncertainty  in  its 
estimates. 
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In  this  scenario,  the  reason  the  vehicle  decides  to  head  toward  the  features  with 
less  uncertainty  associated  with  their  position  is  directly  related  to  the  second  and 
third  convergence  theorems  described  in  Section  2.5.  As  successive  observations  are 
made,  the  uncertainty  in  feature  location  diminishes  to  the  uncertainty  that  was 
present  in  the  vehicle  at  the  time  that  feature  was  first  observed.  Thus,  the  vehicle 
chooses  to  steer  toward  features  having  relatively  small  uncertainties. 


Figure  4-11:  Planned  Perception,  Scenario  1.  True  vehicle  path  versus  estimated  vehicle 
path.  The  red  crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the  estimated 
vehicle  path.  The  minor  jumps  in  estimated  and  true  position  occur  when  the  vehicle 
obtains  an  update  and  improves  its  estimated  position. 


Heading  Error  (rad) 


Figure  4-12:  Planned  Perception,  Scenario  1.  The  errors  of  the  state  estimate 
performing  planned  perception.  The  actual  errors  are  represented  by  the  red  dashe' 
The  solid  blue  lines  represent  ±lcr  error  bounds. 
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Figure  4-13:  Planned  Perception,  Scenario  1.  The  variances  in  x  and  y  position  are  plotted. 
The  dotted  red  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As 
expected,  these  variances  converge  to  lower  limits. 
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Close-up  of  Determinant 


Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 


Figure  4-14:  Planned  Perception,  Scenario  1.  Top  left:  Determinant  of  vehicle 
uncertainty,  The  black  line  represents  the  threshold  when  planned  perception 
was  performed.  Top  right:  The  uncertainty  is  ciuickly  restored  below  the  threshold 
as  a  result  of  the  planned  perception  algorithm.  The  close-up  also  shows  that  in  the 
limit,  as  successive  observations  are  taken,  the  vehicle  uncertainty  converges  to  the 
initial  uncertainty  of  the  vehicle  at  the  time  it  first  observed  a  feature.  This  is  also 
below  the  threshold.  Bottom:  The  same  is  shown  for  the  x  and  y  uncertainty  of  the 
vehicle,  P^j,. 
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Close-up  of  Feature  IDs 

Figure  4-15:  Planned  Perception,  Scenario  1.  Top:  This  figure  shows  the  IDs  of  the 
features  the  vehicle  chose  to  steer  toward  while  performing  planned  perception.  The 
IDs  are  obtained  as  the  feature  is  mapped  (i.e.  the  first  feature  mapped  gets  ID  #1). 
The  feature  ID  is  0  when  the  vehicle  is  “exploring”  and  the  uncertainty  is  below  the 
given  threshold.  Bottom:  A  close-up  of  the  feature  ID  figure  is  shown  here. 
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Planned  perception  -  Scenario  2 

This  scenario  addresses  the  second  criteria  of  planned  perception.  Referring  to  Equa¬ 
tion  4.5,  this  scenario  only  address  ||xy;  —  x„||.  Thus,  the  weighting  gains  a  and  are 
set  to  zero. 

In  this  scenario,  planned  perception  is  performed  by  minimizing  the  distance 
W^fi  -  x„||.  Therefore,  while  performing  planned  perception,  the  vehicle  steers  to¬ 
ward  the  closest,  previously  mapped  feature.  Thus,  when  the  threshold  uncertainty 
determinant  is  exceeded,  the  vehicle  steers  toward  feature  i  that  satisfies 

R  = /3||%-x„||,  (4.9) 

where  /?  is  set  to  1  to  accurately  compare  the  distance  \\xfi  —  Xt,||. 

Deviating  from  the  desired  path  allows  the  vehicle  to  re-observe  features  that 
otherwise  would  not  have  been  in  the  sensor’s  field  of  view.  In  this  case,  the  vehicle 
steers  toward  the  closest  feature  previously  mapped. 

Because  the  constraint  is  to  steer  toward  the  feature  closest  in  distance,  the  vehicle 
does  not  always  diminish  its  uncertainty  through  the  re-observation  of  feature  i  that 
satisfies  Equation  4.9.  Heading  toward  and  re-observing  the  closest  feature  sometimes 
causes  the  vehicle  to  “circle”  a  feature,  re-observing  it  time  and  time  again.  Thus,  the 
uncertainty  does  not  always  achieve  the  desired  reduction  through  the  re-observation 
of  feature  i.  The  “circling”  of  features  can  be  shown  in  Figure  4-16. 

The  vehicle  is  able  to  reduce  its  uncertainty  below  the  given  threshold,  not  always 
through  the  re-observation  of  the  closest  feature,  but  through  re-observing  another 
feature  while  it  is  “circling.”  This  is  shown  in  Figure  4-20  and  Figure  4-16  where 
the  vehicle  is  constantly  steering  toward  and  circling  a  certain  feature.  The  vehicle 
happens  to  re-observe  another  feature  in  the  process  of  circling  the  desired  feature 
i.  The  re-observation  of  another  feature,  along  with  re-observing  the  feature  closest 
to  the  vehicle,  provide  the  update  needed  to  drive  vehicle  uncertainty  below  the 
threshold. 

Figure  4-19  represents  the  breaching  of  the  uncertainty  threshold.  The  threshold 
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is  plotted  with  the  current  uncertainty.  Figure  4-18  shows  the  variances  of  x  and  y 
vehicle  position.  As  successive  re-observations  of  featuresa  are  made,  these  variances 
converge  as  expected. 

The  criteria  set  forth  in  this  scenario  does  not  always  provide  the  necessary  means 
to  achieve  the  desired  vehicle  uncertainty  level.  This  is  due  to  the  vehicle  steer¬ 
ing  toward  the  mapped  feature  that  is  closest  in  distance.  It  is  not  always  the  re¬ 
observation  of  the  closest  feature  i  that  provides  the  necessary  update  information; 
it  is  re-observing  feature  i  along  with  re-observing  a  different  feature  in  the  process 
of  successive  acts  of  circling,  that  provide  the  necessary  measurements  to  successfully 
reduce  vehicle  uncertainty 


Figure  4-16:  Planned  Perception,  Scenario  2.  True  vehicle  path  versus  estimated  vehicle 
path.  The  red  crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the  estimated 
vehicle  path.  The  loops  in  the  path  show  where  the  vehicle  ^‘circles’’  a  feature. 
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Actual  Errors  with  1  sigma  Bounds 


Figure  4-17:  Planned  Perception,  Scenario  2.  The  errors  of  the  state  estimate  when 
performing  planned  perception.  The  actual  errors  are  represented  by  the  red  dashed  line. 
The  solid  blue  lines  represent  ±la  error  bounds. 
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Figure  4-18:  Planned  Perception,  Scenario  2.  The  variances  in  x  and  y  position  are  plotted. 
The  red  dotted  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As 
expected,  these  variances  converge  to  lower  limits. 
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Determinant  of  Vehicle  Uncertainty  Close-up  of  Determinant 


Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 

Figure  4-19;  Planned  Perception,  Scenario  2.  Top  left:  Determinant  of  vehicle  un¬ 
certainty,  P„„,  The  black  line  represents  the  threshold  when  planned  perception  was 
performed.  Top  right:  The  overall  uncertainty  is  quickly  restored  below  the  thresh¬ 
old  as  a  result  of  the  planned  perception  algorithm.  The  close-up  also  shows  that  in 
the  limit,  as  successive  observations  are  taken,  the  vehicle  uncertainty  converges  to 
the  initial  uncertainty  of  the  vehicle  at  the  time  it  first  observed  a  feature.  This  is 
also  below  the  threshold.  Bottom:  The  same  is  shown  for  the  x  and  y  uncertainty 
of  the  vehicle,  P^y. 
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ID  of  Fealures  Steering  Toward  in  Planned  Perception 


IDs  of  Features  Vehicle  Chose  to  Steer  Toward 


Figure  4-20:  Top:  Planned  Perception,  Scenario  2.  This  figure  shows  the  IDs  of 
the  features  the  vehicle  chose  to  steer  toward  while  performing  planned  perception. 
The  IDs  are  obtained  as  the  feature  is  mapped  (i.e.  the  first  feature  mapped  gets  ID 
#1).  The  feature  ID  is  0  when  the  vehicle  is  “exploring”  and  the  uncertainty  is  below 
the  given  threshold.  Bottom:  A  close-up  of  the  feature  ID  figure  is  shown  here. 
The  close-up  shows  how  the  vehicle  continually  circles  a  feature  until  the  uncertainty 
decreases  below  the  given  threshold. 


96 


SIMULATION  DESIGN  AND  RESULTS 


Planned  perception  -  Scenario  3 

This  scenario  combines  all  three  criteria  of  planned  perception.  Thus,  the  feature  the 
vehicle  chooses  to  steer  toward  while  performing  planned  perception  is  determined  by- 
evaluating 


Pi  =  -cv|  (WiSiWf )„  I  +  P\\y.fi  -  x„||  +  C  •  f  {density fi),  (4.10) 


based  on  the  metric 


Ai*  =  arQi  min  Pj, 


(4.11) 


AiPk)  <  AiPk  -  1).  (4.12) 

Since  all  three  criteria  discussed  in  Chapter  3  are  being  considered  simultaneously, 
Fi  must  be  scaled  by  the  weighting  factors  to  obtain  a  scalar  number  that  accurately 
weights  all  three  factors  according  to  the  desired  mission.  For  this  scenario,  the 
weighting  factor  ^  is  scaled  to  20.  Thus,  the  factor  weights  Equation  4.10  so  that  the 
vehicle  will  be  more  likely  to  steer  away  from  any  feature  that  has  another  feature 
within  10  meters  of  it.  This  criteria  is  set  to  minimize  the  problems  inherent  in  data 
association.  The  weighting  gains  a  and  /3  are  set  to  10^^  and  2,  respectively.  These 
gains  affect  the  weighting  of  the  criteria: 

1.  For  all  mapped  features  i  =  1, . . . ,  Nf,  which  re-observation  of  feature  i  will  best 
improve  vehicle  estimates? 

2.  What  is  the  cost  of  re-observing  feature  i  ? 

The  values  of  the  gains  were  chosen  so  as  to  weight  each  criteria  equally.  The  val¬ 
ues  were  obtained  through  analyzing  values  obtained  by  evaluating  Equation  4.10  in 
simulation. 

Figure  4-21  shows  the  path  the  vehicle  travelled.  The  vehicle  deviates  from  the 
desired  path  in  order  to  re-observe  features  and  reduce  the  estimated  uncertainty. 
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The  path  travelled  reflects  the  criteria  for  this  scenario;  the  vehicle  steers  toward  a 
combination  of;  features  with  less  certainty,  features  that  are  relatively  close  to  the 
current  vehicle  position,  and  features  not  located  in  cluttered  areas.  This  is  reflected 
in  Figure  4-25  as  the  vehicle  steers  toward  a  mixture  of  features. 

By  performing  planned  perception,  the  uncertainty  of  the  vehicle  is  desired  to  be 
held  below  a  given  threshold.  Figure  4-24  shows  the  vehicle  uncertainty  and  the  given 
threshold.  This  is  also  reflected  in  the  errors  associated  with  each  state  estimate. 
Figure  4-22,  and  the  variance  of  the  vehicle’s  x  and  y  estimates.  Figure  4-23.  As 
the  vehicle  uncertainty  exceeds  the  threshold,  the  vehicle  maneuvers  to  re-observe 
features. 

Through  the  combination  of  all  three  criteria,  the  vehicle  attempts  to  maintain  a 
desired  uncertainty  level  by  re-observing  features.  The  action  of  the  robot  is  given  by 
evaluating  Equation  4.11  using  the  metric  Equation  4.12.  The  vehicle  steers  towards 
the  feature  which  satisfies  the  above  criteria.  The  features  are  chosen  based  on  a 
tradeoff  between  all  three  criteria.  The  features  chosen  are  a  combination  of  features 
that  are  relatively  close  to  the  vehicle,  features  not  located  in  a  cluttered  area,  and 
features  having  relatively  small  uncertainties. 
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Figure  4-21:  Planned  Perception,  Scenario  3.  True  vehicle  path  versus  estimated  vehicle 
path.  The  red  crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the  estimated 
vehicle  path. 
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Figure  4-22:  Planned  Perception,  Scenario  3.  The  errors  of  the  state  estimate  when 
performing  planned  perception.  The  actual  errors  are  represented  by  the  red  dashed  line. 
The  solid  blue  lines  represent  ±lcr  error  bounds. 
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Figure  4-23:  Planned  Perception,  Scenario  3.  The  variances  in  x  and  y  position  are  plotted. 
The  red  dotted  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As 
expected,  these  variances  converge  to  lower  limits. 
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Determinant  of  Vehicle  Uncertainty  Close-up  of  Determinant 
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Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 

Figure  4-24:  Planned  Perception,  Scenario  3.  Top  left:  Determinant  of  vehicle 
uncertainty,  P^-  The  black  line  represents  the  threshold  when  planned  perception 
was  performed.  Top  right:  The  close-up  shows  that  in  the  limit,  as  successive 
observations  are  taken,  the  vehicle  uncertainty  converges  to  the  initial  uncertainty  of 
the  vehicle  at  the  time  it  first  observed  a  feature.  The  vehicle  uncertainty  is  quickly 
restored  below  the  threshold  as  a  result  of  the  planned  perception  algorithm.  Bottom 
left:  Determinant  of  vehicle  x  and  y  uncertainty,  P^y.  Bottom  Right:  As  expected, 
the  uncertainty  converges. 
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Close-up  of  Feature  IDs 


Figure  4-25:  Planned  Perception,  Scenario  3.  Top:  This  figure  shows  the  IDs  of  the 
features  the  vehicle  chose  to  steer  toward  while  performing  planned  perception.  The 
IDs  are  obtained  as  the  feature  is  mapped  (i.e.  the  first  feature  mapped  gets  ID  #1). 
The  feature  ID  is  0  when  the  vehicle  is  “exploring”  and  the  uncertainty  is  below  the 
given  threshold.  Bottom:  A  close-up  of  the  feature  ID  figure  is  shown  here.  The 
vehicle  chooses  to  steer  toward  a  combination  of  features  that  have  relatively  little 
uncertainty  (those  mapped  earlier)  and  features  that  are  close  to  its  current  position. 
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4.2.4  Planned  perception  simulation  ^2 

This  simulation  integrate  the  planned  perception  and  CML.  Planned  perception  sim¬ 
ulation  #1,  described  in  Section  4.2.3,  constrains  the  overall  uncertainty  associated 
with  the  vehicle  states,  P„„.  This  simulation,  however,  constrains  only  the  uncertainty 
associated  with  the  vehicle’s  x  and  y  estimates,  termed  P^y.  This  is  chosen  because 
it  specifically  constrains  the  area  of  uncertainty  in  the  vehicle’s  x  and  y  position 
estimates. 

The  vehicle  desires  to  navigate  from  waypoint  to  waypoint.  The  planned  percep¬ 
tion  algorithm  goes  into  effect  when  the  determinant  of  P^y  exceeds  a  given  threshold 
P xythreshoid-  This  threshold  determinant  is  set  to  4.0.  The  number  4.0  is  chosen  to 
reflect  the  desire  to  maintain  the  vehicle’s  x  and  y  uncertainty  below  4  m^.  This  sim¬ 
ulation  is  broken  down  into  different  scenarios.  The  following  scenarios  demonstrate 
different  strategies  of  integrating  planned  perception  and  CML  when  constraining 
the  error  uncertainty  for  the  x  and  y  state  estimates.  Except  for  the  determinant 
threshold  constraining  P^y  and  being  set  to  4.0,  the  scenarios  are  the  same  as  those 
in  Section  4.2.3. 

Planned  perception  -  Scenario  1 

This  scenario  addresses  the  first  criteria  of  planned  perception.  Thus,  this  scenario 
only  addresses  |  (WiSiWf)„  |  in  Equation  4.5.  The  weighting  gains  /5  and  C  are  set 
to  zero  and  a  is  set  to  10^^  as  in  Section  4.2.3. 

Figure  4-26  shows  the  trajectories  of  the  true  and  estimated  vehicle  paths.  When 
the  vehicle’s  x  and  y  uncertainty  exceed  the  given  threshold,  the  vehicle  deviates 
from  the  desired  path  to  re-observe  features.  The  re-observation  of  features  allows 
the  vehicle  to  decrease  its  uncertainty  in  the  x  and  y  estimated  states.  In  turn,  this 
also  decreases  the  overall  vehicle  uncertainty,  P,™.  This  can  be  seen  in  Figure  4-29. 

While  performing  planned  perception,  the  vehicle  steers  towards  features  as  shown 
in  Figure  4-30.  As  expected,  these  features  tend  to  be  those  mapped  earlier  during  the 
mission.  This  is  because  these  features  have  more  certainty  in  their  position  estimates 
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and  will  provide  better  updates  for  vehicle  state  estimation. 

The  uncertainty  threshold  is  breached  mainly  during  the  vehicle’s  first  cycle  of 
the  “lawnmower”  pattern.  This  is  shown  in  Figure  4-26  and  also  in  the  close-up  of 
Figure  4-30.  As  the  close-up  shows,  the  vehicle  performs  in  localization  mode  mainly 
during  the  first  run  of  the  pattern  (up  to  around  900  seconds).  The  vehicle  then  re¬ 
observes  features  reducing  uncertainty  estimates.  After  closing-the-loop,  the  vehicle 
mainly  operates  in  exploration  mode  for  the  rest  of  the  mission. 

As  described  in  Section  4.2.3  scenario  1,  these  results  are  consistent  with  the 
convergence  theorems  described  in  Section  2.5. 


Figure  4-26:  Planned  Perception,  Scenario  1.  True  vehicle  path  versus  estimated  vehicle 
path.  The  red  crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the  estimated 
vehicle  path.  The  minor  jumps  in  estimated  and  true  position  occur  when  the  vehicle 
obtains  an  update  and  improves  its  estimated  position. 
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Actual  Errors  with  1  sigma  Bounds 


Figure  4-27;  Planned  Perception,  Scenario  1.  The  errors  of  the  state  estimate  when 
performing  planned  perception.  The  actual  errors  are  represented  by  the  red  dashed  line. 
The  solid  blue  lines  represent  ±la  error  bounds.  As  expected,  these  errors  are  bounded. 
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Figure  4-28:  Planned  Perception,  Scenario  1.  The  variances  in  x  and  y  position  are  plotted. 
The  red  dotted  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As 
expected,  these  variances  converge  to  lower  limits. 
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Determinant  of  Vehicle  Uncertainty  Close-up  of  Determinant 


Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 

Figure  4-29:  Planned  Perception,  Scenario  1.  Top  left:  Determinant  of  vehicle 
uncertainty,  P^-y.  Top  right:  The  close-up  shows  the  vehicle  uncertainty  converging 
to  the  initial  vehicle  uncertainty.  Bottom  Left:  The  determinant  of  the  vehicle 
X  and  y  uncertainty,  The  black  line  represents  the  threshold  when  planned 

perception  was  performed.  Bottom  Right:  The  uncertainty  is  quickly  restored 
below  the  threshold  as  a  result  of  the  planned  perception  algorithm.  The  close-up 
also  shows  that  in  the  limit,  as  successive  observations  are  taken,  the  x  and  y  vehicle 
uncertainty  converges  to  the  initial  x,y  uncertainty  of  the  vehicle  at  the  time  it  first 
observed  a  feature.  This  is  also  below  the  threshold. 
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Figure  4-30;  Planned  Perception,  Scenario  1.  Top:  This  figure  shows  the  IDs  of  the 
features  the  vehicle  chose  to  steer  toward  while  performing  planned  perception.  The 
IDs  are  obtained  as  the  feature  is  mapped  (i.e.  the  first  feature  mapped  gets  ID  #1). 
The  feature  ID  is  0  when  the  vehicle  is  “exploring”  and  the  uncertainty  is  below  the 
given  threshold.  Bottom:  A  close-up  of  the  feature  ID  figure  is  shown  here. 
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Planned  perception  -  Scenario  2 

In  this  scenario,  planned  perception  is  performed  by  minimizing  the  distance  1|%  — 
x„||.  Thus,  when  the  threshold  uncertainty  determinant  is  exceeded,  the  vehicle  steers 
toward  feature  i  that  satisfies  Equation  4.5.  The  weighting  gain  /?  is  set  to  1  while 
the  gains  a  and  C  are  set  to  zero. 

While  performing  planned  perception,  the  vehicle  heads  toward  the  closest  feature. 
The  vehicle  steers  to  re-observe  this  feature  to  decrease  its  estimated  uncertainty. 
However,  because  of  this  constraint,  heading  toward  and  re-observing  the  closest 
feature  sometimes  causes  the  vehicle  to  “circle”  a  feature  as  described  in  Section  4.2.3. 
The  circling  of  features  can  be  shown  in  Figure  4-31. 

The  vehicle  reduces  its  uncertainty  not  always  through  the  re-observation  of  the 
closest  feature,  but  through  re-observing  another  feature  while  it  is  “circling.”  Fig¬ 
ure  4-35  and  Figure  4-31  show  the  vehicle  constantly  steering  toward  and  circling  a 
feature. 

During  its  first  “lap”  of  the  desired  path,  the  vehicle  tends  to  operate  mostly 
in  localization  mode  (planned  perception).  However,  after  re-observing  one  of  the 
first  features  mapped  (closing-the-loop) ,  the  vehicle’s  x  and  y  uncertainty  diminishes 
below  the  threshold.  Confident  in  its  x  and  y  estimates,  the  vehicle  then  spends 
most  of  the  mission  operating  in  exploration  mode.  This  is  seen  through  Figure  4-35; 
around  time  1300  seconds,  the  vehicle  explores. 
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Figure  4-31:  Planned  Perception,  Scenario  2.  True  vehicle  path  versus  estimated  vehicle 
path.  The  red  crosses  represent  the  true  vehicle  path.  The  blue  dots  follow  the  estimated 
vehicle  path.  The  minor  jumps  in  estimated  and  true  position  occur  when  the  vehicle 
obtains  an  update  and  improves  its  estimated  position.  The  loops  in  the  path  show  where 
the  vehicle  “circles”  a  feature. 
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Actual  Errors  with  1  sigma  Bounds 


Figure  4-32:  Planned  Perception,  Scenario  2.  The  errors  of  the  state  estimate  when 
performing  planned  perception.  The  actual  errors  are  represented  by  the  red  dashed  line. 
The  solid  blue  lines  represent  ±lcr  error  bounds.  As  expected,  these  errors  are  bounded. 


Variance  (m  ) 


SIMULATION  DESIGN  AND  RESULTS 


Variance  in  X  and  Y  Vehicle  Position 


time  (s) 


Figure  4~33:  Planned  Perception,  Scenario  2.  The  variances  in  x  and  y  position  are  plotted. 
The  red  dotted  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As 
expected,  these  variances  converge  to  lower  limits. 
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Determinant  of  Vehicle  Uncertainty  Close-up  of  Determinant 


Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 

Figure  4-34:  Planned  Perception,  Scenario  2.  Top  left:  Determinant  of  vehicle 
uncertainty,  P^,;.  Top  right:  The  close-up  shows  the  vehicle  uncertainty  converging 
to  the  initial  vehicle  uncertainty.  Bottom  Left:  The  determinant  of  the  vehicle 
X  and  y  uncertainty,  Pxy  The  black  line  represents  the  threshold  when  planned 
perception  was  performed.  Bottom  Right:  The  uncertainty  is  quickly  restored 
below  the  threshold  as  a  result  of  the  planned  perception  algorithm.  The  close-up 
also  shows  that  in  the  limit,  as  successive  observations  are  taken,  the  x  and  y  vehicle 
uncertainty  converges  to  the  initial  x,y  uncertainty  of  the  vehicle  at  the  time  it  first 
observed  a  feature.  This  is  also  below  the  threshold. 
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Close~up  of  Feature  IDs 

Figure  4-35:  Planned  Perception,  Scenario  2.  Top:  This  figure  shows  the  IDs  of 
the  features  the  vehicle  chose  to  steer  toward  while  performing  planned  perception. 
The  IDs  are  obtained  as  the  feature  is  mapped  (i.e.  the  first  feature  mapped  gets  ID 
^1).  The  feature  ID  is  0  when  the  vehicle  is  “exploring”  and  the  uncertainty  is  below 
the  given  threshold.  Bottom:  A  close-up  of  the  feature  ID  figure  is  shown  here. 
The  close-up  shows  how  the  vehicle  continually  circles  a  feature  until  the  uncertainty 
decreases  below  the  given  threshold. 
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Planned  Perception  -  Scenario  3 

This  scenario  combines  all  three  criteria  of  planned  perception.  The  action  of  the 
robot  is  given  by  evaluating  Equation  4.11.  For  this  scenario,  the  weighting  gains  a, 
/3,  and  (  are  set  to  10^^  2,  and  20,  respectively.  The  assignment  of  these  values  is 
discussed  in  Section  4.2.3. 

The  path  the  vehicle  travelled  is  shown  in  Figure  4-36.  The  vehicle  deviates 
from  the  desired  path  to  re-observe  features  and  reduce  the  uncertainty  in  the  state 
estimates.  The  path  travelled  and  features  chosen  to  steer  toward  reflect  the  criteria 
for  this  scenario;  while  performing  planned  perception,  the  vehicle  steers  toward  a 
feature  that  is  a  combination  of:  position  uncertainty,  distance  to  vehicle,  and  feature 
density.  This  is  shown  in  Figure  4-40. 

Planned  perception  is  performed  to  maintain  a  desired  vehicle  uncertainty.  In  this 
simulation,  the  constraint  is  placed  on  the  uncertainty  in  the  vehicle’s  x  and  y  position. 
Figure  4-39  shows  the  determinant  of  vehicle  uncertainties  with  the  given  threshold. 
The  desire  to  maintain  a  given  uncertainty  threshold  is  reflected  in  Figure  4-37,  which 
shows  the  errors  associated  with  each  vehicle  state  estimate,  and  Figure  4-23,  which 
shows  the  variance  of  the  vehicle’s  x  and  y  estimates. 


SIMULATION  DESIGN  AND  RESULTS 


Heajding  Error  (rad)  Y  Error  (m)  X  Error  (m) 


4.2  SIMULATION  RESULTS 


iir 


Actual  Errors  with  1  sigma  Bounds 


Figure  4-37:  Planned  Perception,  Scenario  3.  The  errors  of  the  state  estimate  when 
performing  planned  perception.  The  actual  errors  are  represented  by  the  red  dashed  line. 
The  solid  blue  lines  represent  dilcr  error  bounds.  As  expected,  these  errors  are  bounded. 
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Figure  4-38;  Planned  Perception,  Scenario  3.  The  variances  in  x  and  y  position  are  plotted. 
The  red  dotted  line  is  the  variance  in  x  while  the  dashed  blue  line  is  the  variance  in  y.  As 
expected,  these  variances  converge  to  lower  limits. 
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Determinant  of  Vehicle  Uncertainty  Close-up  of  Determinant 


Determinant  of  Vehicle  x,y  Uncertainty  Close-up  of  Determinant 

Figure  4-39:  Planned  Perception,  Scenario  3.  Top  left:  Determinant  of  vehicle 
uncertainty,  Top  right:  The  close-up  shows  the  vehicle  uncertainty  converging 
to  the  initial  vehicle  uncertainty.  Bottom  Left:  The  determinant  of  the  vehicle 
X  and  y  uncertainty,  P^^y.  The  black  line  represents  the  threshold  when  planned 
perception  was  performed.  Bottom  Right:  The  uncertainty  is  quickly  restored 
below  the  threshold  as  a  result  of  the  planned  perception  algorithm.  The  close-up 
also  shows  that  in  the  limit,  as  successive  observations  are  taken,  the  x  and  y  vehicle 
uncertainty  converges  to  the  initial  x,y  uncertainty  of  the  vehicle  at  the  time  it  first 
observed  a  feature.  This  is  also  below  the  threshold. 
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Figure  4-40:  Planned  Perception,  Scenario  3.  Top:  This  figure  shows  the  IDs  of  the 
features  the  vehicle  chose  to  steer  toward  while  performing  planned  perception.  The 
IDs  are  obtained  as  the  feature  is  mapped  (i.e.  the  first  feature  mapped  gets  ID  #1). 
The  feature  ID  is  0  when  the  vehicle  is  “exploring”  and  the  uncertainty  is  below  the 
given  threshold.  Bottom:  A  close-up  of  the  feature  ID  figure  is  shown  here. 
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4.3  Conclusions 

Navigating  by  dead  reckoning  alone  requires  projecting  the  vehicle  through  the  ve¬ 
hicle  model.  There  are  no  observations  of  features  to  “reset”  the  vehicle  estimate; 
no  reference  can  be  made  to  reset  the  uncertainty  in  vehicle  position.  Thus,  while 
navigating  by  dead  reckoning,  the  absolute  position  error  for  dead  reckoning  grows 
without  bounds  as  a  result  of  process  noise.  The  unbounded  error  growth  in  state 
estimates  are  shown  in  Figure  4-4.  Dead  reckoning  causes  the  uncertainty  associated 
with  the  vehicle’s  covariance  matrix  to  grow  exponentially  as  shown  in  Figure  4-6. 

Concurrent  mapping  and  localization  is  feature-based  navigation.  The  observation 
of  features  is  used  to  obtain  localization  points  for  navigation.  CML  provides  a  means 
to  navigate  so  the  error  estimates  remain  bounded.  This  can  be  seen  in  Figure  4-8  and 
Figure  4-9.  CML  also  provides  a  method  for  vehicle  uncertainty  to  converge  to  the 
initial  uncertainty  of  the  vehicle  through  successive  re-observation  of  features.  This  is 
shown  in  Figure  4-10.  As  depicted  in  Figure  4-7,  CML  provides  a  more  robust  way  to 
estimate  the  vehicle  path  when  compared  to  dead  reckoning.  Thus,  when  navigating 
from  waypoint  to  waypoint,  CML  is  a  substantial  improvement  to  dead  reckoning. 

Planned  perception  aims  to  improve  the  CML  framework.  As  can  be  seen  in 
Figure  4-10,  the  uncertainty  in  vehicle  estimates  grows  while  performing  a  mission. 
CML  allows  for  the  uncertainty  to  be  reduced  through  the  re-observation  of  features. 
However,  planned  perception  allows  for  this  reduction  to  take  place  quicker  and  in 
a  more  robust  manner.  Planned  perception  constrains  vehicle  uncertainty.  When 
the  uncertainty  exceeds  a  certain  threshold,  the  vehicle  maneuvers  to  re-obtain  a 
reduction  in  the  estimated  uncertainty. 

The  first  simulation  integrating  planned  perception  and  CML  focuses  on  constrain¬ 
ing  the  overall  uncertainty  of  the  vehicle,  The  first  scenario  in  Section  4.2.3  in¬ 
volves  only  addressing  the  first  criteria  in  our  planned  perception  algorithm  described 
in  Chapter  3.  This  scenario  focuses  on  the  re-observation  of  which  feature  will  best 
improve  the  uncertainty  associated  with  vehicle  state  estimates. 

This  scenario  proves  to  improve  CML  performance.  As  shown  in  Figure  4-10  and 
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Figure  4-9,  while  navigating  by  CML  alone,  the  uncertainties  in  vehicle  estimates 
grows  until  the  vehicle  closes-the-loop  or  re-observes  a  feature.  When  planned  per¬ 
ception  is  integrated  in  this  scenario,  CML  performance  is  improved.  This  can  be 
seen  in  Figure  4-14  and  Figure  4-13.  The  vehicle  is  more  confident  in  its  estimates 
than  when  compared  to  navigating  just  by  CML.  Also,  the  overall  uncertainty  in  the 
vehicle  is  improved.  This  is  seen  by  comparing  Figure  4-10  and  Figure  4-14.  Thus, 
integrating  planned  perception  within  CML  further  reduces  estimates  in  vehicle  un¬ 
certainty. 

The  second  scenario  in  Section  4.2.3  minimizes  the  distance  between  the  vehicle 
and  a  pieviously  mapped  feature.  This  scenario  proves  to  be  a  liability  to  achieving 
the  overall  mission  objective  (moving  from  waypoint  to  waypoint).  This  is  because 
as  the  uncertainty  exceeds  the  given  threshold,  the  vehicle  steers  toward  the  mapped 
feature  closest  to  it.  This  may  cause  “circling”  to  occur  as  described  in  Section  4.2.3. 
This  circling  affect  is  undesirable  because  re-observing  this  feature  over  and  over 
may  never  allow  for  the  vehicle  to  reduce  the  estimated  uncertainty  below  the  given 
threshold.  While  circling  in  our  experiments,  it  is  by  chance  that  the  vehicle  happens 
to  re-observe  another  feature  (not  the  one  it  is  circling)  and  is  able  to  successfully 
reduce  its  uncertainty. 

The  third  scenario  combines  all  three  planned  perception  criteria  described  in 
Chapter  3.  This  presents  a  more  realistic  scenario.  This  is  because  autonomous 
vehicles  do  not  have  an  unlimited  power  supply.  Thus,  unlike  operating  as  in  the 
first  scenario,  it  may  not  always  be  desirable  to  travel  far  distances  to  re-observe 
a  certain  feature.  Therefore,  by  incorporating  all  criteria,  a  trade-off  between  each 
is  presented.  The  feature  which  best  satisfies  all  three  criteria  is  the  one  chosen  in 
the  planned  perception  algorithm.  Like  the  first  scenario,  this  simulation  proves  to 
be  an  improvement  to  CML.  The  uncertainty  associated  in  the  vehicle  estimates  is 
improved  as  shown  by  comparing  Figure  4-10  and  Figure  4-24. 

The  integration  of  planned  perception  and  CML  described  in  the  above  scenarios 
provide  methods  to  alter  the  vehicle’s  motion  to  maintain  an  uncertainty  level.  How¬ 
ever,  by  constraining  the  determinant  of  P„„,  the  uncertainty  in  the  x  and  y  position 
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is  still  relatively  large.  This  can  be  seen  in  determinant  figures  and  variance  plots 
of  the  above  scenarios.  Performing  planned  perception  by  constraining  proves 
to  improve  CML.  However,  the  question  is  then  raised  if  it  is  possible  to  obtain  an 
improvement  to  the  vehicle’s  x  and  y  uncertainty. 

The  second  simulation  addresses  the  question  of  constraining  the  uncertainty  in 
the  vehicle’s  x  and  y  estimates;  it  constrains  the  associated  determinant  of  P^j,.  The 
first  scenario  in  Section  4.2.4  proves  to  drastically  improve  CML  performance.  This 
is  shown  in  Figure  4-29.  By  constraining  the  uncertainty  in  the  x  and  y  vehicle 
estimates,  the  uncertainty  in  the  overall  vehicle  estimate,  P„u,  is  also  reduced.  This 
can  be  seen  by  comparing  Figure  4-10  and  Figure  4-29. 

The  second  scenario  in  Section  4.2.4  again  proves  that  performing  planned  per¬ 
ception  by  relying  on  the  re-observation  of  the  nearest  feature  may  not  achieve  the 
mission  objective.  Again,  “circling”  becomes  an  issue.  This  is  seen  by  the  loops  in 
the  vehicle  path  of  Figure  4-31  and  by  analyzing  the  features  the  vehicle  steers  toward 
in  Figure  4-35. 

By  constraining  vehicle  uncertainty,  planned  perception  improves  the  CML  frame¬ 
work.  It  provides  a  method  to  adaptively  alter  the  vehicle’s  sensing  strategy  in  order 
to  maintain  a  better  estimate  of  vehicle  states.  Two  methods  of  constraining  vehicle 
uncertainty  are  introduced.  The  first  involves  constraining  the  overall  uncertainty 
associated  with  the  vehicle,  P^u.  The  second  constrains  the  uncertainty  associated 
with  the  X  and  y  estimates  of  vehicle  position,  P^,,,.  Constraining  P^^  proves  to  be 
more  desirable. 

There  is,  however,  a  trade-off  between  the  two  constraints.  By  constraining  Pxy, 
the  vehicle  spends  most  of  its  “first  lap”  of  the  pattern  operating  in  localization  mode 
performing  planned  perception.  Once  the  vehicle  closes-the-loop,  it  is  able  to  maintain 
a  smaller  uncertainty  level  during  the  rest  of  its  mission.  Thus,  more  energy  is  spent 
localizing  the  vehicle  in  the  beginning  of  the  mission  when  constraining  P^y  After 
closing-the-loop,  the  vehicle  operates  with  more  certainty  compared  to  performing 
planned  perception  constraining  P„„. 

A  trade-off  also  exists  between  the  weighting  gains  and  the  three  different  planned 
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perception  criteria.  Addressing  the  first  criteria  alone  yields  the  best  planned  per¬ 
ception  performance.  However,  it  may  not  always  be  desirable,  due  to  energy  costs, 
to  re-visit  features  that  are  farther  away  than  others.  Also,  only  addressing  the 
second  criteria  proves  to  cause  the  vehicle  to  “circle”  features.  This  circling  and 
re-observation  of  the  same  feature  over  and  over  does  not  always  yield  the  needed 
update  to  reduce  vehicle  uncertainty.  Thus,  by  incorporating  all  three  criteria,  the 
vehicle  no  longer  continually  “circles”  certain  features  and  is  not  required  to  travel 
back  to  the  first  features  mapped.  The  algorithm  allows  for  the  weighting  gains  to 
be  set,  determining  the  performance  of  the  planned  perception  algorithm,  according 
to  the  specific,  desired  mission. 

4.4  Summary 

This  chapter  presented  the  description  of  the  simulation  written  in  (c)  MATLAB 
combining  the  ideas  presented  in  Chapter  2  and  Chapter  3.  The  results  integrating 
planned  perception  within  CML  were  also  presented.  The  results  and  performances 
of  the  different  simulations  were  then  compared. 


Chapter  5 


Conclusions  and  Future  Research 


This  chapter  summarizes  the  contributions  of  this  thesis  and  presents  suggestions  for 
future  research. 


5,1  Contributions 


This  thesis  presents  a  method  for  integrating  planned  perception  within  concurrent 
mapping  and  localization.  Planned  perception  is  the  process  of  adaptively  deter¬ 
mining  the  sensing  strategy  of  the  mobile  robot.  The  goal  of  integrating  planned 
perception  with  CML  is  to  provide  the  mobile  robot  with  a  means  to  determine 
the  optimal  action  given  the  current  knowledge  of  robot  pose,  sensors,  and  the  en¬ 
vironment.  CML  performance,  augmented  with  smarter  sensing  strategies,  exhibits 
improved  performance  by  motivating  changes  in  robot  orientation  and  sensing  strate¬ 
gies.  Our  planned  perception  algorithm  aims  to  minimize  the  uncertainty  associated 
with  the  vehicle’s  state  estimates.  It  provides  a  method  that  addresses  the  trade-off 
between  mission  success  and  knowledge  of  the  vehicle’s  current  state.  Our  approach 
was  applied  and  validated  in  simulation. 
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5.2  Future  Research 

This  thesis  is  motivated  by  the  study  of  autonomous  underwater  vehicles  (AUVs). 
Thus,  a  suggested  area  for  future  research  includes  integrating  planned  perception 
within  CML  in  the  real  world  with  actual  autonomous  robots.  Extending  the  ideas 
presented  in  this  thesis  to  real  world  environments  might  require  insight  and  future 
research  in  the  areas  of  vehicle  modelling,  data  association,  and  navigating  by  CML 
in  dynamic  environments. 

One  extension  to  planned  perception  may  be  to  incorporate  sensor  characteristics 
into  adaptive  sensing  strategies.  Taking  into  account  the  sensor’s  range  and  field 
of  view  may  motivate  the  vehicle  steer  not  towards  certain  features,  but  towards  a 
virtual  waypoint  that  would  allow  the  vehicle  to  observe  certain  features.  Another 
extension  may  be  to  develop  a  policy  that  explicitly  minimizes  angular  error  in  the 
map.  This  may  be  performed  by  analyzing  the  estimated  angular  error  between 
features  and  developing  a  strategy  whose  motion  reduces  this  error. 

Our  approach  provides  a  method  to  minimize  the  uncertainty  associated  in  the 
vehicle  estimates.  An  extension  of  integrating  planned  perception  and  CML  may  be 
to  analyze  a  different  metric  of  performing  adaptive  sensing;  instead  of  utilizing  a 
policy  to  re-observe  features,  utilize  a  policy  that  integrates  adaptive  motion  control 
and  CML  based  on  the  correlation  structure  of  the  covariance  matrix.  Therefore, 
instead  of  choosing  to  re-observe  a  feature  that  reduces  vehicle  uncertainty,  choose 
actions  that  make  the  map  more  fully  correlated. 
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